/***************************************************************************
 创建者: 华磊
 开始时间: 2020.8.14
 copyright: (C) 华友高科
 修改说明: (每次有修改就添加一条，带有 修改人，修改时间，修改描述)
 example (1) hualei 2020.5.8 类的具体实现代码编写

 ***************************************************************************/
#include "lasersafetydetect.h"
#include "domparser.h"
#include <qdebug.h>
#include "motionmessage.h"
#include "robotio.h"

#define D_MAX_INTEGE 2147483647



LaserSafetyDetect::LaserSafetyDetect(int robotIdIn, MotionMessage *motionMessageIn, QString safeFileIn,
                  QString carBorderFileIn, RobotIo *ioControllerIn)
{
    robotId=robotIdIn;
    motionMessage=motionMessageIn;
    carBorderFile=carBorderFileIn;
    ioController=ioControllerIn;

    motionStatus=E_MOTION_ID_NONE;

    minConsumed=100000000000;
    currentConsumed=0;
    maxConsumed=0;

    safeRatio=1.0;
    isDetectEnable=true;
    detectMode=E_BARRIER_TETECT_MODE_ROTATION;
//    isUseSheft=false;
    barrierRunMode=E_BARRIER_RUN_MODE_COMMON;

    slowTime=1.0;

    collisionConfigFile=safeFileIn;

    int tmpReturn;

    QString readComent;
    tmpReturn=readCollisionConfigFile(safeFileIn,collisionInfoAll,isUseNewControlMethod,readComent);
    if(1!=tmpReturn)
    {
        qDebug()<<"error,LaserSafetyDetect readConfigFile";
        addMsg(ENUM_MSG_ERROR, "LaserSafetyDetect", "LaserSafetyDetect", 8141, robotId ,0,0,0,0,readComent);
    }
    else
    {
        initialCollisionParameter(collisionInfoAll);
    }
    tmpReturn=readCarBorderFile(carBorderFileIn);
//    //Y方向初始化
//    sheft_borderB_x.resize(4);
//    sheft_borderB_y.resize(4);
//    sheft_borderB_x[0]=sheft_borderA_y[0];
//    sheft_borderB_y[0]=sheft_borderA_x[0];

//    sheft_borderB_x[1]=-sheft_borderA_y[1];
//    sheft_borderB_y[1]=-sheft_borderA_x[1];

//    sheft_borderB_x[2]=sheft_borderA_y[2];
//    sheft_borderB_y[2]=sheft_borderA_x[2];

//    sheft_borderB_x[3]=-sheft_borderA_y[3];
//    sheft_borderB_y[3]=-sheft_borderA_x[3];

    if(1!=tmpReturn)
    {
        qDebug()<<"error,LaserSafetyDetect readCarBorderFile";
        addMsg(ENUM_MSG_ERROR, "LaserSafetyDetect", "LaserSafetyDetect", 8140, robotId );
        return ;
    }


    vehicleBorderId=0;
    mutex.lock();
    calculateCarOffsetRegion();
    calculateStopRegion();
    mutex.unlock();



}

int LaserSafetyDetect::controlTypeSwitch(int controlTypeIn)
{
    controlType=(EN_CONTROL_TYPE)controlTypeIn;
    return 1;
}

int LaserSafetyDetect::setVehicleDirection(bool isPositiveIn)
{
    isMovePositive=isPositiveIn;
    return 1;
}

int LaserSafetyDetect::setMotionStatus(E_MOTION_ID statusIn)
{
    motionStatus=statusIn;
    return 1;
}

int LaserSafetyDetect::getCurrentVehicleBorderId()
{
    return vehicleBorderId;
}

int LaserSafetyDetect::setCurrentVehicleBorderId(int idIn)
{
    qDebug()<<"setCurrentVehicleBorderId"<<idIn;
    if(0==idIn)
    {
        calculateCarOffsetRegion();

    }
    else if(1==idIn)
    {
        calculateCarOffsetRegion(sheft_borderA_x,sheft_borderA_y);

    }
    else
    {
        calculateCarOffsetRegion(sheft_borderB_x,sheft_borderB_y);
    }
    vehicleBorderId=idIn;
    return 1;
}

int LaserSafetyDetect::getAgvBorderInfo(VehicleBorderInfo &infoOut)
{
    mutex.lock();
    infoOut.x=origin_car_border_x;
    infoOut.y=origin_car_border_y;
    infoOut.offset_front=front_offset;
    infoOut.offset_back=back_offset;
    infoOut.offset_left=left_offset;
    infoOut.offset_right=right_offset;
    mutex.unlock();
    return 1;
}

int LaserSafetyDetect::setAgvBorderRectInfo(BorderRectInfo rectInfoIn, int &returnFlag)
{
        returnFlag=writeAgvBorderRectInfo(rectInfoIn);
        readCarBorderFile(carBorderFile);
        setCurrentVehicleBorderId(vehicleBorderId);
        return 1;
}

int LaserSafetyDetect::getAgvSheftInfo(int sheftIndex,BorderRectInfo &infoOut)
{
    mutex.lock();
    if(0==sheftIndex)
    {
        infoOut.x=sheft_borderA_x;
        infoOut.y=sheft_borderA_y;
    }
    else
    {
        infoOut.x=sheft_borderB_x;
        infoOut.y=sheft_borderB_y;
    }

    mutex.unlock();
    return 1;
}

int LaserSafetyDetect::setAgvSheftRectInfo(int sheftIndex, BorderRectInfo rectInfoIn, int &returnFlag)
{
    returnFlag=writeAgvSheftRectInfo(sheftIndex,rectInfoIn);
    readCarBorderFile(carBorderFile);
    setCurrentVehicleBorderId(vehicleBorderId);

    return 1;
}

E_BARRIER_RUN_MODE LaserSafetyDetect::getBarrierRunMode()
{
    return barrierRunMode;
}

int LaserSafetyDetect::setBarrierRunMode(E_BARRIER_RUN_MODE modeIn)
{
//    if(E_BARRIER_RUN_MODE_SHEFT_X==modeIn)
//    {
////        setUseSheftRegion(true);
//        calculateCarOffsetRegion(sheft_borderA_x,sheft_borderA_y);
//    }
//    else if(E_BARRIER_RUN_MODE_SHEFT_Y==modeIn)
//    {
////        std::vector<double> tmp_border_x, tmp_border_y;
////        tmp_border_x.push_back(sheft_border_x.back());
////        tmp_border_y.push_back(sheft_border_y.back());
////        for(int i=0;i<sheft_border_x.size()-1;i++)
////        {
////            tmp_border_x.push_back(sheft_border_x[i]);
////            tmp_border_y.push_back(sheft_border_y[i]);
////        }
////        setUseSheftRegion(true);
//        calculateCarOffsetRegion(sheft_borderB_x,sheft_borderB_y);
//    }
//    else
//    {
////        setUseSheftRegion(false);
//        calculateCarOffsetRegion();
//    }
    barrierRunMode=modeIn;
    return 1;
}

int LaserSafetyDetect::getCollisionParameter(double &barrierSlowDownDistance_XOut, double &barrierSlowDownRatioOut,
                                             double &barrierStopDistanceXOut,double &barrierStopDistanceYOut)
{
    barrierSlowDownDistance_XOut=barrierSlowDownDistance_x;
    barrierSlowDownRatioOut=barrierSlowDownRatio;
    barrierStopDistanceXOut=barrierStopDistance_x;
    barrierStopDistanceYOut=barrierStopDistance_x;
    return 1;
}


int LaserSafetyDetect::getSafeCollisionInfo(int sensorType, CollisionPara &infoOut)
{
    if(0==sensorType)
    {
        infoOut.barrierSlowDownDistance_x=barrierSlowDownDistance_x;
        infoOut.barrierSlowDownDistance_y=barrierSlowDownDistance_y;
        infoOut.barrierStopDistance_x=barrierStopDistance_x;
        infoOut.barrierStopDistance_y=barrierStopDistance_y;
        infoOut.pieExtendedDistance=pieExtendedDistance;
        infoOut.stepSafetyRatioList=adjustPara;
    }
    else if(1==sensorType)
    {
        infoOut.barrierSlowDownDistance_x=barrierSlowDownDistance_x_vmark;
        infoOut.barrierSlowDownDistance_y=barrierSlowDownDistance_y_vmark;
        infoOut.barrierStopDistance_x=barrierStopDistance_x_vmark;
        infoOut.barrierStopDistance_y=barrierStopDistance_y_vmark;
        infoOut.pieExtendedDistance=pieExtendedDistance_vmark;
        infoOut.stepSafetyRatioList=adjustPara_vmark;

    }
    else if(2==sensorType)
    {
        infoOut.barrierSlowDownDistance_x=barrierSlowDownDistance_x_narrowA;
        infoOut.barrierSlowDownDistance_y=barrierSlowDownDistance_y_narrowA;
        infoOut.barrierStopDistance_x=barrierStopDistance_x_narrowA;
        infoOut.barrierStopDistance_y=barrierStopDistance_y_narrowA;
        infoOut.pieExtendedDistance=pieExtendedDistance_narrowA;
        infoOut.stepSafetyRatioList=adjustPara_narrowA;

    }
    else if(3==sensorType)
    {
        infoOut.barrierSlowDownDistance_x=barrierSlowDownDistance_x_narrowB;
        infoOut.barrierSlowDownDistance_y=barrierSlowDownDistance_y_narrowB;
        infoOut.barrierStopDistance_x=barrierStopDistance_x_narrowB;
        infoOut.barrierStopDistance_y=barrierStopDistance_y_narrowB;
        infoOut.pieExtendedDistance=pieExtendedDistance_narrowB;
        infoOut.stepSafetyRatioList=adjustPara_narrowB;

    }
    return 1;
}

int LaserSafetyDetect::setSafeCollisionInfo(int sensorType, CollisionPara infoIn, int &returnFlag)
{
    if(0==sensorType)
    {
        collisionInfoAll.barrierSlowDownDistance_x=infoIn.barrierSlowDownDistance_x;
        collisionInfoAll.barrierSlowDownDistance_y=infoIn.barrierSlowDownDistance_y;
        collisionInfoAll.barrierStopDistance_x=infoIn.barrierStopDistance_x;
        collisionInfoAll.barrierStopDistance_y=infoIn.barrierStopDistance_y;
        collisionInfoAll.pieExtendedDistance=infoIn.pieExtendedDistance;
        collisionInfoAll.stepSafetyRatioList=infoIn.stepSafetyRatioList;
    }
    else if(1==sensorType)
    {
        collisionInfoAll.barrierSlowDownDistance_x_vmark=infoIn.barrierSlowDownDistance_x;
        collisionInfoAll.barrierSlowDownDistance_y_vmark=infoIn.barrierSlowDownDistance_y;
        collisionInfoAll.barrierStopDistance_x_vmark=infoIn.barrierStopDistance_x;
        collisionInfoAll.barrierStopDistance_y_vmark=infoIn.barrierStopDistance_y;
        collisionInfoAll.pieExtendedDistance_vmark=infoIn.pieExtendedDistance;
        collisionInfoAll.stepSafetyRatioList_vmark=infoIn.stepSafetyRatioList;

    }
    else if(2==sensorType)
    {
        collisionInfoAll.barrierSlowDownDistance_x_narrowA=infoIn.barrierSlowDownDistance_x;
        collisionInfoAll.barrierSlowDownDistance_y_narrowA=infoIn.barrierSlowDownDistance_y;
        collisionInfoAll.barrierStopDistance_x_narrowA=infoIn.barrierStopDistance_x;
        collisionInfoAll.barrierStopDistance_y_narrowA=infoIn.barrierStopDistance_y;
        collisionInfoAll.pieExtendedDistance_narrowA=infoIn.pieExtendedDistance;
        collisionInfoAll.stepSafetyRatioList_narrowA=infoIn.stepSafetyRatioList;

    }
    else if(3==sensorType)
    {
        collisionInfoAll.barrierSlowDownDistance_x_narrowB=infoIn.barrierSlowDownDistance_x;
        collisionInfoAll.barrierSlowDownDistance_y_narrowB=infoIn.barrierSlowDownDistance_y;
        collisionInfoAll.barrierStopDistance_x_narrowB=infoIn.barrierStopDistance_x;
        collisionInfoAll.barrierStopDistance_y_narrowB=infoIn.barrierStopDistance_y;
        collisionInfoAll.pieExtendedDistance_narrowB=infoIn.pieExtendedDistance;
        collisionInfoAll.stepSafetyRatioList_narrowB=infoIn.stepSafetyRatioList;

    }
    initialCollisionParameter(collisionInfoAll);
    mutex.lock();
    setBarrierRunMode(barrierRunMode);//更新的壁障区域，货架也会叠加偏移
    mutex.unlock();
    returnFlag= writeCollisionConfigFile(collisionConfigFile,collisionInfoAll);
    return returnFlag;
}

//int LaserSafetyDetect::setUseSheftRegion(bool isUseIn)
//{
////    isUseSheft=isUseIn;
//    return 1;
//}


int LaserSafetyDetect::getTimeConsumed(double &minConsumedOut, double &currentConsumedOut, double &maxConsumedOut)
{
    minConsumedOut=minConsumed;
    currentConsumedOut=currentConsumed;
    maxConsumedOut=maxConsumed;
    return 1;
}


int LaserSafetyDetect::getCarBorder(std::vector<VehiclePosition> &carBorderOut)
{
    mutex.lock();
    carBorderOut.resize(origin_car_border_x.size());
    VehiclePosition tmpPos;
    for(int i=0;i<origin_car_border_x.size();i++)
    {
        tmpPos.x=origin_car_border_x[i];
        tmpPos.y=origin_car_border_y[i];
        carBorderOut[i]=tmpPos;
    }
    mutex.unlock();
    return 1;

}

int LaserSafetyDetect::getCarBorderOffset(std::vector<VehiclePosition> &carBorderOffsetOut)
{
    mutex.lock();
    carBorderOffsetOut.resize(origin_car_border_x.size());
    VehiclePosition tmpPos;
    for(int i=0;i<validBorder_x.size();i++)
    {
        tmpPos.x=validBorder_x[i];
        tmpPos.y=validBorder_y[i];
        carBorderOffsetOut[i]=tmpPos;
    }
    mutex.unlock();
    return 1;
}

E_BARRIER_TETECT_MODE LaserSafetyDetect::getDetectMode()
{
    return detectMode;

}

int LaserSafetyDetect::getPointCloudSafeStatus(std::vector<string> &sensorNameOut, std::vector<double> &sensorSafeRatioOut,
                                               bool &isCheckEnableOut)
{
    mutex.lock();
    isCheckEnableOut=isDetectEnable;
    sensorNameOut=sensorName;
    sensorSafeRatioOut=sensorSafeRatio;
    mutex.unlock();

    return 1;
}

int LaserSafetyDetect::setPointCloudCheckEnable(bool isOn)
{
    isDetectEnable=isOn;
    return 1;
}



double LaserSafetyDetect::getSafetyRatioOut()
{
//    return 1; //for debug
    if(false==isDetectEnable)
    {
        return 1;
    }
    return safeRatio;
}

double LaserSafetyDetect::getSafetySlowTime()
{
    return slowTime;
}

int LaserSafetyDetect::setVehicleVelocity(VehicleVelocity velIn)
{
    vehicleVel=velIn;
    return 1;
}

int LaserSafetyDetect::setPointCloudData(const QVector<PointCloudData_3D> &pointDataIn)
{
    //判断点云是否有更新．
    static unsigned long long timeStamp[10];

    bool isChanged=false;
    for(int i=0;i<pointDataIn.size();i++)
    {
        if(timeStamp[i]!=pointDataIn[i].timeStamp)
        {
            isChanged=true;
            timeStamp[i]=pointDataIn[i].timeStamp;
        }

    }
    if(false==isChanged)
    {
        return 0;
    }

    //时间分析
    struct timespec startTime,endTime;
    clock_gettime(CLOCK_MONOTONIC, &startTime);

    if(false==isDetectEnable)
    {
        return -1;
    }



    mutex.lock();
    updateDetectRegion(vehicleVel);//?coredump debug test3

    //直接计算结果，不做缓存
    int cloudSize=pointDataIn.size();
    std::vector<double> safeRatioList;
    safeRatioList.resize(cloudSize);

    sensorName.clear();
    sensorSafeRatio.clear();
    double tmpRatio1=1.0;
    for(int i=0;i<cloudSize;i++)
    {

        calculatePointCloudData(pointDataIn[i],safeRatioList[i]);//debug test2？
        if(tmpRatio1>safeRatioList[i])
        {
            tmpRatio1=safeRatioList[i];
        }
        timeStamp[i]=pointDataIn[i].timeStamp;


    }
    for(int i=0;i<cloudSize;i++)
    {
        sensorName.push_back(pointDataIn[i].deviceName);
        sensorSafeRatio.push_back(safeRatioList[i]);
    }
    //io激光雷达壁障逻辑处理
    if(isUseIoLaser)
    {
        double tmpIoMin=1;
        for(int i=0;i<ioLaserConfigList.size();i++)
        {
            double tmpSafeRatioOut;
            calculateIolaserBarrierLogic(detectMode,ioLaserConfigList[i],tmpSafeRatioOut);
            //将最后的数据替换为io的检测数据.默认传递进来了４个点云设备．
//            sensorName.push_back(ioLaserConfigList[i].laserName);
//            sensorSafeRatio.push_back(tmpSafeRatioOut);
//            sensorName.back()=ioLaserConfigList[i].laserName;
//            sensorSafeRatio.back()=tmpSafeRatioOut;

            if(tmpRatio1>tmpSafeRatioOut)
            {
                tmpRatio1=tmpSafeRatioOut;
            }
            if(tmpIoMin>tmpSafeRatioOut)
            {
                tmpIoMin=tmpSafeRatioOut;
            }
        }
        sensorName.back()="ioLaser";
        sensorSafeRatio.back()=tmpIoMin;
    }

    safeRatio=tmpRatio1;

    mutex.unlock();



    clock_gettime(CLOCK_MONOTONIC, &endTime);
    currentConsumed=1000000000*(endTime.tv_sec-startTime.tv_sec)+endTime.tv_nsec-startTime.tv_nsec;
    currentConsumed=currentConsumed/1000.0;
    if(currentConsumed<minConsumed)
    {
        minConsumed=currentConsumed;
    }
    if(currentConsumed>maxConsumed)
    {
        maxConsumed=currentConsumed;
    }


    return 1;

}

int LaserSafetyDetect::writeAgvBorderRectInfo(BorderRectInfo rectInfoIn)
{

    DomParser xmlParser;

    bool ok = xmlParser.openXml(carBorderFile, QIODevice::ReadWrite );
    if( !ok )//无法打开文件！
    {
       qDebug()<<"error, saveVehicleBorderFile open file error！";
       return -1;
    }


    //车边框读取
    QDomNode  inNode = xmlParser.findSubNode(xmlParser.getRootDomElement(), "BasicSetting", ok );
    if( !ok )
    {

        qDebug()<<"error, readCarBorderFile node error！";
        return -1;
    }

    QDomNodeList devNodes = inNode.childNodes();


    if(rectInfoIn.x.size()!=devNodes.size())
    {
        qDebug()<<"error, readCarBorderFile node error！rectInfoIn.x.size()!=devNodes.size()";
        return -8;
    }

    for(int i=0;i<rectInfoIn.x.size();i++)
    {
        QDomNode devND =  devNodes.at(i);
        xmlParser.writeXml( xmlParser.findSubNode( devND, "x_position", ok ), rectInfoIn.x[i] );
        if(!ok)
        {
           return -100-i;
        }
        xmlParser.writeXml( xmlParser.findSubNode( devND, "y_position", ok ), rectInfoIn.y[i] );
        if(!ok)
        {
           return -200-i;
        }
    }

    xmlParser.closeXml(true);
    return 1;
}

int LaserSafetyDetect::writeAgvSheftRectInfo(int sheftIndex, BorderRectInfo rectInfoIn)
{
    DomParser xmlParser;

    bool ok = xmlParser.openXml(carBorderFile, QIODevice::ReadWrite );
    if( !ok )//无法打开文件！
    {
       qDebug()<<"error, setAgvSheftRectInfo open file error！";
       return -1;
    }


    //车边框读取
    QDomNode  inNode;
    if(0==sheftIndex)
    {
        inNode= xmlParser.findSubNode(xmlParser.getRootDomElement(), "SheftBorderA", ok );
    }
    else
    {
        inNode= xmlParser.findSubNode(xmlParser.getRootDomElement(), "SheftBorderB", ok );
    }
    if( !ok )
    {

        qDebug()<<"error, readCarBorderFile node error！";
        return -1;
    }

    QDomNodeList devNodes = inNode.childNodes();


    if(rectInfoIn.x.size()!=devNodes.size())
    {
        qDebug()<<"error, readCarBorderFile node error！rectInfoIn.x.size()!=devNodes.size()";
        return -8;
    }

    for(int i=0;i<rectInfoIn.x.size();i++)
    {
        QDomNode devND =  devNodes.at(i);
        xmlParser.writeXml( xmlParser.findSubNode( devND, "x_position", ok ), rectInfoIn.x[i] );
        if(!ok)
        {
           return -100-i;
        }
        xmlParser.writeXml( xmlParser.findSubNode( devND, "y_position", ok ), rectInfoIn.y[i] );
        if(!ok)
        {
           return -200-i;
        }
    }


    xmlParser.closeXml(true);
    return 1;
}



int LaserSafetyDetect::setVehicleBorderOffset(double front_offsetIn, double back_offsetIn, double left_offsetIn, double right_offsetIn)
{
    front_offset=front_offsetIn;
    back_offset=back_offsetIn;
    left_offset=left_offsetIn;
    right_offset=right_offsetIn;
//    saveVehicleBorderFile(carBorderFile);
    mutex.lock();
    setCurrentVehicleBorderId(vehicleBorderId);//更新的壁障区域，货架也会叠加偏移
    mutex.unlock();
    return 1;
}

int LaserSafetyDetect::calculateIolaserBarrierLogic(E_BARRIER_TETECT_MODE detectModeIn,const IoLaserPara &ioLaserIn,
                                                    double &safeRatioOut)
{
    safeRatioOut=1.0;
    //检测区域设定
    switch(barrierRunMode)
    {
    case E_BARRIER_RUN_MODE_COMMON:
    {
        if(0==vehicleBorderId)
        {
            ioController->setDo(ioLaserIn.laserDo1_index,false);
            ioController->setDo(ioLaserIn.laserDo2_index,false);
            ioController->setDo(ioLaserIn.laserDo3_index,false);
            ioController->setDo(ioLaserIn.laserDo4_index,false);
        }
        else if(1==vehicleBorderId)
        {
            ioController->setDo(ioLaserIn.laserDo1_index,false);
            ioController->setDo(ioLaserIn.laserDo2_index,true);
            ioController->setDo(ioLaserIn.laserDo3_index,false);
            ioController->setDo(ioLaserIn.laserDo4_index,false);
        }
        else if(2==vehicleBorderId)
        {
            ioController->setDo(ioLaserIn.laserDo1_index,false);
            ioController->setDo(ioLaserIn.laserDo2_index,false);
            ioController->setDo(ioLaserIn.laserDo3_index,true);
            ioController->setDo(ioLaserIn.laserDo4_index,false);
        }

        break;
    }
    case E_BARRIER_RUN_MODE_VMARK:
    {
        if(0==vehicleBorderId)
        {
            ioController->setDo(ioLaserIn.laserDo1_index,true);
            ioController->setDo(ioLaserIn.laserDo2_index,false);
            ioController->setDo(ioLaserIn.laserDo3_index,false);
            ioController->setDo(ioLaserIn.laserDo4_index,false);
        }
        else if(1==vehicleBorderId)
        {
            ioController->setDo(ioLaserIn.laserDo1_index,true);
            ioController->setDo(ioLaserIn.laserDo2_index,true);
            ioController->setDo(ioLaserIn.laserDo3_index,false);
            ioController->setDo(ioLaserIn.laserDo4_index,false);
        }
        else if(2==vehicleBorderId)
        {
            ioController->setDo(ioLaserIn.laserDo1_index,true);
            ioController->setDo(ioLaserIn.laserDo2_index,false);
            ioController->setDo(ioLaserIn.laserDo3_index,true);
            ioController->setDo(ioLaserIn.laserDo4_index,false);
        }

        break;
    }

    case E_BARRIER_RUN_MODE_NARROW_B:
    {
        if(0==vehicleBorderId)
        {
            ioController->setDo(ioLaserIn.laserDo1_index,0);
            ioController->setDo(ioLaserIn.laserDo2_index,0);
            ioController->setDo(ioLaserIn.laserDo3_index,0);
            ioController->setDo(ioLaserIn.laserDo4_index,1);
        }
        else if(1==vehicleBorderId)
        {
            ioController->setDo(ioLaserIn.laserDo1_index,0);
            ioController->setDo(ioLaserIn.laserDo2_index,1);
            ioController->setDo(ioLaserIn.laserDo3_index,0);
            ioController->setDo(ioLaserIn.laserDo4_index,1);
        }
        else if(2==vehicleBorderId)
        {
            ioController->setDo(ioLaserIn.laserDo1_index,0);
            ioController->setDo(ioLaserIn.laserDo2_index,0);
            ioController->setDo(ioLaserIn.laserDo3_index,1);
            ioController->setDo(ioLaserIn.laserDo4_index,0);
        }

        break;
    }

    case E_BARRIER_RUN_MODE_NARROW_A:
    {
        if(0==vehicleBorderId)
        {
            ioController->setDo(ioLaserIn.laserDo1_index,1);
            ioController->setDo(ioLaserIn.laserDo2_index,1);
            ioController->setDo(ioLaserIn.laserDo3_index,1);
            ioController->setDo(ioLaserIn.laserDo4_index,0);
        }
        else if(1==vehicleBorderId)
        {
            ioController->setDo(ioLaserIn.laserDo1_index,1);
            ioController->setDo(ioLaserIn.laserDo2_index,0);
            ioController->setDo(ioLaserIn.laserDo3_index,0);
            ioController->setDo(ioLaserIn.laserDo4_index,1);
        }
        else if(2==vehicleBorderId)
        {
            ioController->setDo(ioLaserIn.laserDo1_index,1);
            ioController->setDo(ioLaserIn.laserDo2_index,1);
            ioController->setDo(ioLaserIn.laserDo3_index,0);
            ioController->setDo(ioLaserIn.laserDo4_index,1);
        }

        break;
    }

    }

    if(E_BARRIER_TETECT_MODE_NONE==detectModeIn)
    {
        safeRatioOut=1.0;
        return 1;
    }

    if(E_BARRIER_TETECT_MODE_ROTATION==detectModeIn &&  false==ioLaserIn.isRotateValid)
    {
        safeRatioOut=1.0;
        return 1;
    }

    //检测结果处理
    switch(ioLaserIn.laserLocationType)
    {
    case E_LOCATION_TYPE_BACK:
    {
        if(E_BARRIER_TETECT_MODE_FRONT==detectModeIn)
        {
            safeRatioOut=1.0;
            return 1;
        }
        else
        {
            return ioLaserOutputLogic(ioLaserIn,safeRatioOut);

        }
        break;
    }
    case E_LOCATION_TYPE_FRONT:
    {
        if(E_BARRIER_TETECT_MODE_BACK==detectModeIn)
        {
            safeRatioOut=1.0;
            return 1;
        }
        else
        {

            return ioLaserOutputLogic(ioLaserIn,safeRatioOut);
        }
        break;
    }
    case E_LOCATION_TYPE_LEFT:
    {
        if(E_BARRIER_TETECT_MODE_FRONT==detectModeIn || E_BARRIER_TETECT_MODE_BACK==detectModeIn
                || E_BARRIER_TETECT_MODE_RIGHT==detectModeIn || E_BARRIER_TETECT_MODE_FRONT_RIGHT==detectModeIn
                || E_BARRIER_TETECT_MODE_BACK_RIGHT==detectModeIn )
        {
            safeRatioOut=1.0;
            return 1;
        }
        else
        {

            return ioLaserOutputLogic(ioLaserIn,safeRatioOut);
        }
        break;
    }
    case E_LOCATION_TYPE_RIGHT:
    {
        if(E_BARRIER_TETECT_MODE_FRONT==detectModeIn || E_BARRIER_TETECT_MODE_BACK==detectModeIn
                ||E_BARRIER_TETECT_MODE_LEFT==detectModeIn ||E_BARRIER_TETECT_MODE_FRONT_LEFT==detectModeIn
                ||E_BARRIER_TETECT_MODE_BACK_LEFT==detectModeIn)
        {
            safeRatioOut=1.0;
            return 1;
        }
        else
        {

            return ioLaserOutputLogic(ioLaserIn,safeRatioOut);
        }
        break;
    }
    }


    return 1;
}

int LaserSafetyDetect::ioLaserOutputLogic(const IoLaserPara &ioLaserIn, double &safeRatioOut)
{
    if(false==ioLaserIn.isLowValid)//高电平
    {
        if(ioController->getDi(ioLaserIn.laserDi1_index))//逻辑电平是什么高有效吗？
        {
            safeRatioOut=0.0;
            return 1;
        }
        else if(ioController->getDi(ioLaserIn.laserDi2_index)  && ioLaserIn.laserOutputCount>1)
        {
            safeRatioOut=barrierSlowDownRatio;
            return 1;
        }
        else if(ioController->getDi(ioLaserIn.laserDi3_index) && ioLaserIn.laserOutputCount>2)
        {
            safeRatioOut=barrierSlowDownRatio+0.15;
            return 1;
        }
    }
    else
    {
        if(false==ioController->getDi(ioLaserIn.laserDi1_index))//逻辑电平是什么高有效吗？
        {
            safeRatioOut=0.0;
            return 1;
        }
        else if(false==ioController->getDi(ioLaserIn.laserDi2_index) && ioLaserIn.laserOutputCount>1)
        {
            safeRatioOut=barrierSlowDownRatio;
            return 1;
        }
        else if(false==ioController->getDi(ioLaserIn.laserDi3_index) && ioLaserIn.laserOutputCount>2)
        {
            safeRatioOut=barrierSlowDownRatio+0.15;
            return 1;
        }
    }

    return 1;
}

int LaserSafetyDetect::calculateDetectModeByVel(VehicleVelocity velIn)
{
    double transThreholdVel=0.03;
    double rotateThreholdVel=0.1;
    if(velIn.x_move>=transThreholdVel && fabs(velIn.z_rotate)<rotateThreholdVel && fabs(velIn.y_move)<transThreholdVel)//效果不好，低速旋转时，可能有前进或者后退的分量，导致检测不好。
    {
        detectMode=E_BARRIER_TETECT_MODE_FRONT;
    }
    else if(velIn.x_move<=-transThreholdVel && fabs(velIn.z_rotate)<rotateThreholdVel && fabs(velIn.y_move)<transThreholdVel)
    {
        detectMode=E_BARRIER_TETECT_MODE_BACK;
    }
    else if(fabs(velIn.x_move)<transThreholdVel && fabs(velIn.y_move)<transThreholdVel)//?
    {
        detectMode=E_BARRIER_TETECT_MODE_ROTATION;
    }
    else if(velIn.y_move>=transThreholdVel && fabs(velIn.z_rotate)<rotateThreholdVel && fabs(velIn.x_move)<transThreholdVel)
    {
        detectMode=E_BARRIER_TETECT_MODE_LEFT;
    }
    else if(velIn.y_move<=-transThreholdVel && fabs(velIn.z_rotate)<rotateThreholdVel&& fabs(velIn.x_move)<transThreholdVel )
    {
        detectMode=E_BARRIER_TETECT_MODE_RIGHT;
    }
    else if(velIn.x_move>=transThreholdVel && fabs(velIn.z_rotate)<rotateThreholdVel && velIn.y_move>=transThreholdVel)
    {
        detectMode=E_BARRIER_TETECT_MODE_FRONT_LEFT;
    }
    else if(velIn.x_move>=transThreholdVel && fabs(velIn.z_rotate)<rotateThreholdVel && velIn.y_move<=-transThreholdVel)
    {
        detectMode=E_BARRIER_TETECT_MODE_FRONT_RIGHT;
    }
    else if(velIn.x_move<=-transThreholdVel && fabs(velIn.z_rotate)<rotateThreholdVel && velIn.y_move>=transThreholdVel)
    {
        detectMode=E_BARRIER_TETECT_MODE_BACK_LEFT;
    }
    else if(velIn.x_move<=-transThreholdVel && fabs(velIn.z_rotate)<rotateThreholdVel && velIn.y_move<=-transThreholdVel)
    {
        detectMode=E_BARRIER_TETECT_MODE_BACK_RIGHT;
    }
    else
    {
        detectMode=E_BARRIER_TETECT_MODE_ROTATION;
    }


    calculateAdjustPara(velIn);





    return 1;
}

int LaserSafetyDetect::calculateAdjustPara(VehicleVelocity velIn)
{
    //根据速度修正减速探测距离,和减速比
    double composedVel=sqrt(velIn.x_move*velIn.x_move+velIn.y_move*velIn.y_move);
    switch(barrierRunMode)
    {
    case E_BARRIER_RUN_MODE_COMMON:
    {
       updateSlowDownDistance(composedVel,adjustPara);
       break;
    }
    case E_BARRIER_RUN_MODE_VMARK:
    {
       updateSlowDownDistance(composedVel,adjustPara_vmark);
       break;
    }
    case E_BARRIER_RUN_MODE_NARROW_A:
    {
       updateSlowDownDistance(composedVel,adjustPara_narrowA);
       break;
    }
    case E_BARRIER_RUN_MODE_NARROW_B:
    {
       updateSlowDownDistance(composedVel,adjustPara_narrowB);
       break;
    }
    }
    return 1;
}

int LaserSafetyDetect::calculateDetectModeByMotionStatus(VehicleVelocity velIn, E_MOTION_ID motionStatusIn,
                                                         bool isMovePositiveIn)
{
    switch(motionStatusIn)
    {
    case E_MOTION_ID_NONE:
    {
        return calculateDetectModeByVel(velIn);
        break;
    }
    case E_MOTION_ID_ROTATE:
    {
        detectMode=E_BARRIER_TETECT_MODE_ROTATION;
        break;
    }
    case E_MOTION_ID_MOVE_LINE:
    {
        double transThreholdVel=0.03;
        if(isMovePositiveIn)
        {
            if(velIn.x_move>=transThreholdVel  && velIn.y_move>=transThreholdVel)
            {
                detectMode=E_BARRIER_TETECT_MODE_FRONT_LEFT;
            }
            else if(velIn.x_move>=transThreholdVel  && velIn.y_move<=-transThreholdVel)
            {
                detectMode=E_BARRIER_TETECT_MODE_FRONT_RIGHT;
            }
            else
            {
                detectMode=E_BARRIER_TETECT_MODE_FRONT;
            }
        }
        else
        {
            if(velIn.x_move<=-transThreholdVel  && velIn.y_move>=transThreholdVel)
            {
                detectMode=E_BARRIER_TETECT_MODE_BACK_LEFT;
            }
            else if(velIn.x_move<=-transThreholdVel  && velIn.y_move<=-transThreholdVel)
            {
                detectMode=E_BARRIER_TETECT_MODE_BACK_RIGHT;
            }
            else
            {
                detectMode=E_BARRIER_TETECT_MODE_BACK;
            }
        }


        break;
    }
    case E_MOTION_ID_MOVE_WAIT_STEERING:
    {
        detectMode=E_BARRIER_TETECT_MODE_NONE;
        break;
    }
    }

    calculateAdjustPara(velIn);
    return 1;
}

int LaserSafetyDetect::updateDetectRegion(VehicleVelocity vehicleVelocityIn)
{
    if(controlType<=ENUM_CONTROL_T2 || false==isUseNewControlMethod)
    {
        calculateDetectModeByVel(vehicleVelocityIn);
    }
    else
    {
        calculateDetectModeByMotionStatus(vehicleVelocityIn,motionStatus,isMovePositive);
    }

    switch(detectMode)
    {
    case E_BARRIER_TETECT_MODE_NONE:
    {
        calculateCeaseRegion_rotation(0,0);
        calculateStopRegion(detectMode);
        break;
    }
    case E_BARRIER_TETECT_MODE_FRONT:
    {
        calculateCeaseRegion_front();
        calculateStopRegion(detectMode);
        break;
    }
    case E_BARRIER_TETECT_MODE_BACK:
    {
        calculateCeaseRegion_back();
        calculateStopRegion(detectMode);
        break;
    }
    case E_BARRIER_TETECT_MODE_LEFT:
    {
        calculateCeaseRegion_left();
        calculateStopRegion(detectMode);
        break;
    }
    case E_BARRIER_TETECT_MODE_RIGHT:
    {
        calculateCeaseRegion_right();
        calculateStopRegion(detectMode);
        break;
    }
    case E_BARRIER_TETECT_MODE_ROTATION:
    {
        calculateCeaseRegion_rotation();
        calculateStopRegion();
        break;
    }
    case E_BARRIER_TETECT_MODE_FRONT_LEFT:
    {
        calculateCeaseRegion_front_left();
        calculateStopRegion();
        break;
    }
    case E_BARRIER_TETECT_MODE_FRONT_RIGHT:
    {
        calculateCeaseRegion_front_right();
        calculateStopRegion();
        break;
    }
    case E_BARRIER_TETECT_MODE_BACK_LEFT:
    {
        calculateCeaseRegion_back_left();
        calculateStopRegion();
        break;
    }
    case E_BARRIER_TETECT_MODE_BACK_RIGHT:
    {
        calculateCeaseRegion_back_right();
        calculateStopRegion();
        break;
    }

    }


    return 1;
}

int LaserSafetyDetect::calculateCarOffsetRegion()
{
    validBorder_x.resize(4);
    validBorder_y.resize(4);
    validBorder_x[0]=origin_car_border_x[0]+front_offset;
    validBorder_y[0]=origin_car_border_y[0]+left_offset;
    validBorder_x[1]=origin_car_border_x[1]+front_offset;
    validBorder_y[1]=origin_car_border_y[1]-right_offset;
    validBorder_x[2]=origin_car_border_x[2]-back_offset;
    validBorder_y[2]=origin_car_border_y[2]-right_offset;
    validBorder_x[3]=origin_car_border_x[3]-back_offset;
    validBorder_y[3]=origin_car_border_y[3]+left_offset;
    return 1;

}

int LaserSafetyDetect::calculateCarOffsetRegion(std::vector<double> border_x, std::vector<double> border_y)
{
    validBorder_x.resize(4);
    validBorder_y.resize(4);
    validBorder_x[0]=border_x[0]+front_offset;
    validBorder_y[0]=border_y[0]+left_offset;
    validBorder_x[1]=border_x[1]+front_offset;
    validBorder_y[1]=border_y[1]-right_offset;
    validBorder_x[2]=border_x[2]-back_offset;
    validBorder_y[2]=border_y[2]-right_offset;
    validBorder_x[3]=border_x[3]-back_offset;
    validBorder_y[3]=border_y[3]+left_offset;
    return 1;
}


int LaserSafetyDetect::calculateStopRegion()
{
    return calculateStopRegion(E_BARRIER_TETECT_MODE_ROTATION);
}



int LaserSafetyDetect::calculateStopRegion(E_BARRIER_TETECT_MODE detectModeIn)
{
    if(E_BARRIER_TETECT_MODE_NONE==detectModeIn)
    {
        return calculateStopRegion_para(0,0,0,0);
    }
    switch(barrierRunMode)
    {
//    case E_BARRIER_RUN_MODE_SHEFT_X:
//    case E_BARRIER_RUN_MODE_SHEFT_Y:
    case E_BARRIER_RUN_MODE_COMMON:
    {
       return calculateStopRegion_para(barrierStopDistance_x,barrierStopDistance_x,
                                       barrierStopDistance_y,barrierStopDistance_y);
       break;
    }
    case E_BARRIER_RUN_MODE_VMARK:
    {
        switch(detectModeIn)
        {
        case E_BARRIER_TETECT_MODE_FRONT:
        {
            return calculateStopRegion_para(barrierStopDistance_x_vmark,0,
                                       barrierStopDistance_y_vmark,barrierStopDistance_y_vmark);
            break;
        }
        case E_BARRIER_TETECT_MODE_BACK:
        {
            return calculateStopRegion_para(0,barrierStopDistance_x_vmark,
                                       barrierStopDistance_y_vmark,barrierStopDistance_y_vmark);
            break;
        }
        case E_BARRIER_TETECT_MODE_LEFT:
        {
            return calculateStopRegion_para(barrierStopDistance_x_vmark,barrierStopDistance_x_vmark,
                                       barrierStopDistance_y_vmark,0);
            break;
        }
        case E_BARRIER_TETECT_MODE_RIGHT:
        {
            return calculateStopRegion_para(barrierStopDistance_x_vmark,barrierStopDistance_x_vmark,
                                      0, barrierStopDistance_y_vmark);
            break;
        }
        case E_BARRIER_TETECT_MODE_ROTATION:
        {
            return calculateStopRegion_para(barrierStopDistance_x_vmark,barrierStopDistance_x_vmark,
                                      barrierStopDistance_y_vmark,barrierStopDistance_y_vmark);
            break;
        }
        default:
        {
            return calculateStopRegion_para(barrierStopDistance_x,barrierStopDistance_x,barrierStopDistance_y,barrierStopDistance_y);
        }
        }

       break;
    }
    case E_BARRIER_RUN_MODE_NARROW_A:
    {
        switch(detectModeIn)
        {
        case E_BARRIER_TETECT_MODE_FRONT:
        {
            return calculateStopRegion_para(barrierStopDistance_x_narrowA,0,
                                       barrierStopDistance_y_narrowA,barrierStopDistance_y_narrowA);
            break;
        }
        case E_BARRIER_TETECT_MODE_BACK:
        {
            return calculateStopRegion_para(0,barrierStopDistance_x_narrowA,
                                       barrierStopDistance_y_narrowA,barrierStopDistance_y_narrowA);
            break;
        }
        case E_BARRIER_TETECT_MODE_LEFT:
        {
            return calculateStopRegion_para(barrierStopDistance_x_narrowA,barrierStopDistance_x_narrowA,
                                       barrierStopDistance_y_narrowA,0);
            break;
        }
        case E_BARRIER_TETECT_MODE_RIGHT:
        {
            return calculateStopRegion_para(barrierStopDistance_x_narrowA,barrierStopDistance_x_narrowA,
                                      0, barrierStopDistance_y_narrowA);
            break;
        }
        case E_BARRIER_TETECT_MODE_ROTATION:
        {
            return calculateStopRegion_para(barrierStopDistance_x_narrowA,barrierStopDistance_x_narrowA,
                                      barrierStopDistance_y_narrowA,barrierStopDistance_y_narrowA);
            break;
        }
        default:
        {
            return calculateStopRegion_para(barrierStopDistance_x,barrierStopDistance_x,barrierStopDistance_y,barrierStopDistance_y);
        }
        }

       break;
    }

    case E_BARRIER_RUN_MODE_NARROW_B:
    {
        switch(detectModeIn)
        {
        case E_BARRIER_TETECT_MODE_FRONT:
        {
            return calculateStopRegion_para(barrierStopDistance_x_narrowB,0,
                                       barrierStopDistance_y_narrowB,barrierStopDistance_y_narrowB);
            break;
        }
        case E_BARRIER_TETECT_MODE_BACK:
        {
            return calculateStopRegion_para(0,barrierStopDistance_x_narrowB,
                                       barrierStopDistance_y_narrowB,barrierStopDistance_y_narrowB);
            break;
        }
        case E_BARRIER_TETECT_MODE_LEFT:
        {
            return calculateStopRegion_para(barrierStopDistance_x_narrowB,barrierStopDistance_x_narrowB,
                                       barrierStopDistance_y_narrowB,0);
            break;
        }
        case E_BARRIER_TETECT_MODE_RIGHT:
        {
            return calculateStopRegion_para(barrierStopDistance_x_narrowB,barrierStopDistance_x_narrowB,
                                      0, barrierStopDistance_y_narrowB);
            break;
        }
        case E_BARRIER_TETECT_MODE_ROTATION:
        {
            return calculateStopRegion_para(barrierStopDistance_x_narrowB,barrierStopDistance_x_narrowB,
                                      barrierStopDistance_y_narrowB,barrierStopDistance_y_narrowB);
            break;
        }
        default:
        {
            return calculateStopRegion_para(barrierStopDistance_x,barrierStopDistance_x,barrierStopDistance_y,barrierStopDistance_y);
        }
        }

       break;
    }

    }
    return 1;
}

int LaserSafetyDetect::calculateStopRegion_para(double frontOffset, double backOffset,
                                           double leftOffset, double rightOffset)
{
    zone_stop_x.resize(4);
    zone_stop_y.resize(4);
    zone_stop_x[0]=validBorder_x[0]+frontOffset;
    zone_stop_y[0]=validBorder_y[0]+leftOffset;
    zone_stop_x[1]=validBorder_x[1]+frontOffset;
    zone_stop_y[1]=validBorder_y[1]-rightOffset;
    zone_stop_x[2]=validBorder_x[2]-backOffset;
    zone_stop_y[2]=validBorder_y[2]-rightOffset;
    zone_stop_x[3]=validBorder_x[3]-backOffset;
    zone_stop_y[3]=validBorder_y[3]+leftOffset;
    return 1;
}



int LaserSafetyDetect::calculateCeaseRegion_front()
{
    switch(barrierRunMode)
    {
//    case E_BARRIER_RUN_MODE_SHEFT_X:
//    case E_BARRIER_RUN_MODE_SHEFT_Y:

    case E_BARRIER_RUN_MODE_COMMON:
    {
       return calculateCeaseRegion_front_para(barrierSlowDownDistance_x,pieExtendedDistance,barrierSlowDownDistance_y);
       break;
    }
    case E_BARRIER_RUN_MODE_VMARK:
    {
       return calculateCeaseRegion_front_para(barrierSlowDownDistance_x_vmark,pieExtendedDistance_vmark
                                              ,barrierSlowDownDistance_y_vmark);
       break;
    }
    case E_BARRIER_RUN_MODE_NARROW_A:
    {
       return calculateCeaseRegion_front_para(barrierSlowDownDistance_x_narrowA,pieExtendedDistance_narrowA
                                              ,barrierSlowDownDistance_y_narrowA);
       break;
    }
    case E_BARRIER_RUN_MODE_NARROW_B:
    {
       return calculateCeaseRegion_front_para(barrierSlowDownDistance_x_narrowB,pieExtendedDistance_narrowB
                                              ,barrierSlowDownDistance_y_narrowB);
       break;
    }
    }
    return 0;
}

int LaserSafetyDetect::calculateCeaseRegion_front_para(double barrierSlowDownDistanceIn,double pieExtendedDistanceIn
                                                       ,double besideSlowDownDistanceIn)
{
    zone_slow_x.resize(6);//这个错误找了好久啊，为啥ｖector出界时，ｃｏｒｅdump报的不对啊。
    zone_slow_y.resize(6);

    zone_slow_x[0]=validBorder_x[0]+barrierSlowDownDistanceIn;
    zone_slow_y[0]=validBorder_y[0]+pieExtendedDistanceIn;
    zone_slow_x[1]=validBorder_x[1]+barrierSlowDownDistanceIn;
    zone_slow_y[1]=validBorder_y[1]-pieExtendedDistanceIn;

    zone_slow_x[2]=validBorder_x[1]+besideSlowDownDistanceIn;
    zone_slow_y[2]=validBorder_y[1]-besideSlowDownDistanceIn;
//    zone_slow_x[3]=validBorder_x[2]-besideSlowDownDistanceIn;
    zone_slow_x[3]=validBorder_x[2];
    zone_slow_y[3]=validBorder_y[2]-besideSlowDownDistanceIn;
//    zone_slow_x[4]=validBorder_x[3]-besideSlowDownDistanceIn;
    zone_slow_x[4]=validBorder_x[3];
    zone_slow_y[4]=validBorder_y[3]+besideSlowDownDistanceIn;
    zone_slow_x[5]=validBorder_x[0]+besideSlowDownDistanceIn;
    zone_slow_y[5]=validBorder_y[0]+besideSlowDownDistanceIn;

    return 1;
}

int LaserSafetyDetect::calculateCeaseRegion_back()
{
    switch(barrierRunMode)
    {
//    case E_BARRIER_RUN_MODE_SHEFT_X:
//    case E_BARRIER_RUN_MODE_SHEFT_Y:
    case E_BARRIER_RUN_MODE_COMMON:
    {
       return calculateCeaseRegion_back_para(barrierSlowDownDistance_x,pieExtendedDistance,barrierSlowDownDistance_y);
       break;
    }
    case E_BARRIER_RUN_MODE_VMARK:
    {
       return calculateCeaseRegion_back_para(barrierSlowDownDistance_x_vmark,pieExtendedDistance,barrierSlowDownDistance_y_vmark);
       break;
    }
    case E_BARRIER_RUN_MODE_NARROW_A:
    {
       return calculateCeaseRegion_back_para(barrierSlowDownDistance_x_narrowA,pieExtendedDistance,
                                             barrierSlowDownDistance_y_narrowA);
       break;
    }
    case E_BARRIER_RUN_MODE_NARROW_B:
    {
       return calculateCeaseRegion_back_para(barrierSlowDownDistance_x_narrowB,
                                             pieExtendedDistance,barrierSlowDownDistance_y_narrowB);
       break;
    }
    }
    return 0;
}

int LaserSafetyDetect::calculateCeaseRegion_back_para(double barrierSlowDownDistanceIn,double pieExtendedDistanceIn
                                                      ,double besideSlowDownDistanceIn)
{
    zone_slow_x.resize(6);
    zone_slow_y.resize(6);

    zone_slow_x[0]=validBorder_x[2]-barrierSlowDownDistanceIn;
    zone_slow_y[0]=validBorder_y[2]-pieExtendedDistanceIn;
    zone_slow_x[1]=validBorder_x[3]-barrierSlowDownDistanceIn;
    zone_slow_y[1]=validBorder_y[3]+pieExtendedDistanceIn;

    zone_slow_x[2]=validBorder_x[3]-besideSlowDownDistanceIn;
    zone_slow_y[2]=validBorder_y[3]+besideSlowDownDistanceIn;
//    zone_slow_x[3]=validBorder_x[0]+besideSlowDownDistanceIn;
    zone_slow_x[3]=validBorder_x[0];
    zone_slow_y[3]=validBorder_y[0]+besideSlowDownDistanceIn;
//    zone_slow_x[4]=validBorder_x[1]+besideSlowDownDistanceIn;
    zone_slow_x[4]=validBorder_x[1];
    zone_slow_y[4]=validBorder_y[1]-besideSlowDownDistanceIn;
    zone_slow_x[5]=validBorder_x[2]-besideSlowDownDistanceIn;
    zone_slow_y[5]=validBorder_y[2]-besideSlowDownDistanceIn;

    return 1;
}

int LaserSafetyDetect::calculateCeaseRegion_left()
{
    switch(barrierRunMode)
    {
//    case E_BARRIER_RUN_MODE_SHEFT_X:
//    case E_BARRIER_RUN_MODE_SHEFT_Y:
    case E_BARRIER_RUN_MODE_COMMON:
    {
       return calculateCeaseRegion_left_para(barrierSlowDownDistance_x,pieExtendedDistance,barrierSlowDownDistance_y);
       break;
    }
    case E_BARRIER_RUN_MODE_VMARK:
    {
       return calculateCeaseRegion_left_para(barrierSlowDownDistance_x_vmark,pieExtendedDistance,barrierSlowDownDistance_y_vmark);
       break;
    }
    case E_BARRIER_RUN_MODE_NARROW_A:
    {
       return calculateCeaseRegion_left_para(barrierSlowDownDistance_x_narrowA,
                                             pieExtendedDistance,barrierSlowDownDistance_y_narrowA);
       break;
    }
    case E_BARRIER_RUN_MODE_NARROW_B:
    {
       return calculateCeaseRegion_left_para(barrierSlowDownDistance_x_narrowB,
                                             pieExtendedDistance,barrierSlowDownDistance_y_narrowB);
       break;
    }
    }
    return 0;
}

int LaserSafetyDetect::calculateCeaseRegion_left_para(double barrierSlowDownDistanceIn,double pieExtendedDistanceIn
                                                      ,double besideSlowDownDistanceIn)
{
    zone_slow_x.resize(6);
    zone_slow_y.resize(6);

    zone_slow_x[0]=validBorder_x[3]-pieExtendedDistanceIn;
    zone_slow_y[0]=validBorder_y[3]+barrierSlowDownDistanceIn;
    zone_slow_x[1]=validBorder_x[0]+pieExtendedDistanceIn;
    zone_slow_y[1]=validBorder_y[0]+barrierSlowDownDistanceIn;

    zone_slow_x[2]=validBorder_x[0]+besideSlowDownDistanceIn;
    zone_slow_y[2]=validBorder_y[0]+besideSlowDownDistanceIn;
    zone_slow_x[3]=validBorder_x[1]+besideSlowDownDistanceIn;
    zone_slow_y[3]=validBorder_y[1]-besideSlowDownDistanceIn;
    zone_slow_x[4]=validBorder_x[2]-besideSlowDownDistanceIn;
    zone_slow_y[4]=validBorder_y[2]-besideSlowDownDistanceIn;
    zone_slow_x[5]=validBorder_x[3]-besideSlowDownDistanceIn;
    zone_slow_y[5]=validBorder_y[3]+besideSlowDownDistanceIn;

    return 1;
}

int LaserSafetyDetect::calculateCeaseRegion_right()
{
    switch(barrierRunMode)
    {
//    case E_BARRIER_RUN_MODE_SHEFT_X:
//    case E_BARRIER_RUN_MODE_SHEFT_Y:
    case E_BARRIER_RUN_MODE_COMMON:
    {
       return calculateCeaseRegion_right_para(barrierSlowDownDistance_x,pieExtendedDistance,barrierSlowDownDistance_y);
       break;
    }
    case E_BARRIER_RUN_MODE_VMARK:
    {
       return calculateCeaseRegion_right_para(barrierSlowDownDistance_x_vmark,pieExtendedDistance,barrierSlowDownDistance_y_vmark);
       break;
    }
    case E_BARRIER_RUN_MODE_NARROW_A:
    {
       return calculateCeaseRegion_right_para(barrierSlowDownDistance_x_narrowA,pieExtendedDistance
                                              ,barrierSlowDownDistance_y_narrowA);
       break;
    }
    case E_BARRIER_RUN_MODE_NARROW_B:
    {
       return calculateCeaseRegion_right_para(barrierSlowDownDistance_x_narrowB,
                                              pieExtendedDistance,barrierSlowDownDistance_y_narrowB);
       break;
    }
    }
    return 0;
}

int LaserSafetyDetect::calculateCeaseRegion_right_para(double barrierSlowDownDistanceIn,double pieExtendedDistanceIn
                                                       ,double besideSlowDownDistanceIn)
{
    zone_slow_x.resize(6);
    zone_slow_y.resize(6);

    zone_slow_x[0]=validBorder_x[1]+pieExtendedDistanceIn;
    zone_slow_y[0]=validBorder_y[1]-barrierSlowDownDistanceIn;
    zone_slow_x[1]=validBorder_x[2]-pieExtendedDistanceIn;
    zone_slow_y[1]=validBorder_y[2]-barrierSlowDownDistanceIn;

    zone_slow_x[2]=validBorder_x[2]-besideSlowDownDistanceIn;
    zone_slow_y[2]=validBorder_y[2]-besideSlowDownDistanceIn;
    zone_slow_x[3]=validBorder_x[3]-besideSlowDownDistanceIn;
    zone_slow_y[3]=validBorder_y[3]+besideSlowDownDistanceIn;
    zone_slow_x[4]=validBorder_x[0]+besideSlowDownDistanceIn;
    zone_slow_y[4]=validBorder_y[0]+besideSlowDownDistanceIn;
    zone_slow_x[5]=validBorder_x[1]+besideSlowDownDistanceIn;
    zone_slow_y[5]=validBorder_y[1]-besideSlowDownDistanceIn;

    return 1;
}



int LaserSafetyDetect::calculateCeaseRegion_rotation(double distanceX,double distanceY)
{
    zone_slow_x.resize(4);
    zone_slow_y.resize(4);
    zone_slow_x[0]=validBorder_x[0]+distanceX;
    zone_slow_y[0]=validBorder_y[0]+distanceY;
    zone_slow_x[1]=validBorder_x[1]+distanceX;
    zone_slow_y[1]=validBorder_y[1]-distanceY;
    zone_slow_x[2]=validBorder_x[2]-distanceX;
    zone_slow_y[2]=validBorder_y[2]-distanceY;
    zone_slow_x[3]=validBorder_x[3]-distanceX;
    zone_slow_y[3]=validBorder_y[3]+distanceY;
    return 1;
}

int LaserSafetyDetect::calculateCeaseRegion_rotation()
{
    switch(barrierRunMode)
    {
//    case E_BARRIER_RUN_MODE_SHEFT_X:
//    case E_BARRIER_RUN_MODE_SHEFT_Y:
    case E_BARRIER_RUN_MODE_COMMON:
    {
       return calculateCeaseRegion_rotation(barrierSlowDownDistance_x,barrierSlowDownDistance_y);
       break;
    }
    case E_BARRIER_RUN_MODE_VMARK:
    {
       return calculateCeaseRegion_rotation(barrierSlowDownDistance_x_vmark,barrierSlowDownDistance_y_vmark);
       break;
    }
    case E_BARRIER_RUN_MODE_NARROW_A:
    {
       return calculateCeaseRegion_rotation(barrierSlowDownDistance_x_narrowA,barrierSlowDownDistance_y_narrowA);
       break;
    }
    case E_BARRIER_RUN_MODE_NARROW_B:
    {
       return calculateCeaseRegion_rotation(barrierSlowDownDistance_x_narrowB,barrierSlowDownDistance_y_narrowB);
       break;
    }
    }
    return 0;
}



int LaserSafetyDetect::calculateCeaseRegion_front_left()
{
    switch(barrierRunMode)
    {
//    case E_BARRIER_RUN_MODE_SHEFT_X:
//    case E_BARRIER_RUN_MODE_SHEFT_Y:
    case E_BARRIER_RUN_MODE_COMMON:
    {
       return calculateCeaseRegion_front_left_para(barrierSlowDownDistance_x);
       break;
    }
    case E_BARRIER_RUN_MODE_VMARK:
    {
       return calculateCeaseRegion_front_left_para(barrierSlowDownDistance_x_vmark);
       break;
    }
    case E_BARRIER_RUN_MODE_NARROW_A:
    {
       return calculateCeaseRegion_front_left_para(barrierSlowDownDistance_x_narrowA);
       break;
    }
    case E_BARRIER_RUN_MODE_NARROW_B:
    {
       return calculateCeaseRegion_front_left_para(barrierSlowDownDistance_x_narrowB);
       break;
    }
    }
    return 0;
    return 1;
}

int LaserSafetyDetect::calculateCeaseRegion_front_left_para(double distance)
{
    zone_slow_x.resize(4);
    zone_slow_y.resize(4);

    zone_slow_x[0]=validBorder_x[0]+distance;//
    zone_slow_y[0]=validBorder_y[0]+distance;//
    zone_slow_x[1]=validBorder_x[1]+distance;//
    zone_slow_y[1]=validBorder_y[1]-distance;//
    zone_slow_x[2]=validBorder_x[2]-distance;//
    zone_slow_y[2]=validBorder_y[2]-distance;//
    zone_slow_x[3]=validBorder_x[3]-distance;//
    zone_slow_y[3]=validBorder_y[3]+distance;//
}


int LaserSafetyDetect::calculateCeaseRegion_front_right()
{
    switch(barrierRunMode)
    {
//    case E_BARRIER_RUN_MODE_SHEFT_X:
//    case E_BARRIER_RUN_MODE_SHEFT_Y:
    case E_BARRIER_RUN_MODE_COMMON:
    {
       return calculateCeaseRegion_front_right_para(barrierSlowDownDistance_x);
       break;
    }
    case E_BARRIER_RUN_MODE_VMARK:
    {
       return calculateCeaseRegion_front_right_para(barrierSlowDownDistance_x_vmark);
       break;
    }
    case E_BARRIER_RUN_MODE_NARROW_A:
    {
       return calculateCeaseRegion_front_right_para(barrierSlowDownDistance_x_narrowA);
       break;
    }
    case E_BARRIER_RUN_MODE_NARROW_B:
    {
       return calculateCeaseRegion_front_right_para(barrierSlowDownDistance_x_narrowB);
       break;
    }
    }
    return 0;
    return 1;
}

int LaserSafetyDetect::calculateCeaseRegion_front_right_para(double distance)
{
    zone_slow_x.resize(4);
    zone_slow_y.resize(4);

    zone_slow_x[0]=validBorder_x[0]+distance;
    zone_slow_y[0]=validBorder_y[0]+distance;
    zone_slow_x[1]=validBorder_x[1]+distance;
    zone_slow_y[1]=validBorder_y[1]-distance;
    zone_slow_x[2]=validBorder_x[2]-distance;
    zone_slow_y[2]=validBorder_y[2]-distance;
    zone_slow_x[3]=validBorder_x[3]-distance;
    zone_slow_y[3]=validBorder_y[3]+distance;
    return 1;
}

int LaserSafetyDetect::calculateCeaseRegion_back_left()
{
    switch(barrierRunMode)
    {
//    case E_BARRIER_RUN_MODE_SHEFT_X:
//    case E_BARRIER_RUN_MODE_SHEFT_Y:
    case E_BARRIER_RUN_MODE_COMMON:
    {
       return calculateCeaseRegion_back_left_para(barrierSlowDownDistance_x);
       break;
    }
    case E_BARRIER_RUN_MODE_VMARK:
    {
       return calculateCeaseRegion_back_left_para(barrierSlowDownDistance_x_vmark);
       break;
    }
    case E_BARRIER_RUN_MODE_NARROW_A:
    {
       return calculateCeaseRegion_back_left_para(barrierSlowDownDistance_x_narrowA);
       break;
    }
    case E_BARRIER_RUN_MODE_NARROW_B:
    {
       return calculateCeaseRegion_back_left_para(barrierSlowDownDistance_x_narrowB);
       break;
    }
    }
    return 0;
    return 1;
}

int LaserSafetyDetect::calculateCeaseRegion_back_left_para(double distance)
{
    zone_slow_x.resize(4);
    zone_slow_y.resize(4);

    zone_slow_x[0]=validBorder_x[0]+distance;//
    zone_slow_y[0]=validBorder_y[0]+distance;//
    zone_slow_x[1]=validBorder_x[1]+distance;//
    zone_slow_y[1]=validBorder_y[1]-distance;//
    zone_slow_x[2]=validBorder_x[2]-distance;//
    zone_slow_y[2]=validBorder_y[2]-distance;//
    zone_slow_x[3]=validBorder_x[3]-distance;//
    zone_slow_y[3]=validBorder_y[3]+distance;//
    return 1;
}

int LaserSafetyDetect::calculateCeaseRegion_back_right()
{
    switch(barrierRunMode)
    {
//    case E_BARRIER_RUN_MODE_SHEFT_X:
//    case E_BARRIER_RUN_MODE_SHEFT_Y:
    case E_BARRIER_RUN_MODE_COMMON:
    {
       return calculateCeaseRegion_back_right_para(barrierSlowDownDistance_x);
       break;
    }
    case E_BARRIER_RUN_MODE_VMARK:
    {
       return calculateCeaseRegion_back_right_para(barrierSlowDownDistance_x_vmark);
       break;
    }
    case E_BARRIER_RUN_MODE_NARROW_A:
    {
       return calculateCeaseRegion_back_right_para(barrierSlowDownDistance_x_narrowA);
       break;
    }
    case E_BARRIER_RUN_MODE_NARROW_B:
    {
       return calculateCeaseRegion_back_right_para(barrierSlowDownDistance_x_narrowB);
       break;
    }
    }
    return 0;
    return 1;
}

int LaserSafetyDetect::calculateCeaseRegion_back_right_para(double distance)
{
    zone_slow_x.resize(4);
    zone_slow_y.resize(4);

    zone_slow_x[0]=validBorder_x[0]+distance;
    zone_slow_y[0]=validBorder_y[0]+distance;
    zone_slow_x[1]=validBorder_x[1]+distance;
    zone_slow_y[1]=validBorder_y[1]-distance;
    zone_slow_x[2]=validBorder_x[2]-distance;
    zone_slow_y[2]=validBorder_y[2]-distance;
    zone_slow_x[3]=validBorder_x[3]-distance;
    zone_slow_y[3]=validBorder_y[3]+distance;
    return 1;
}



int LaserSafetyDetect::calculatePointCloudData(const PointCloudData_3D &pointDataIn,double &safeRatioOut)
{
    int validPointNumber=2;//有的地方因为反射会产生多个错误点；
    int tmpCount=0;
    int pointCount=pointDataIn.point_x.size();
    bool isIn;
    bool isInSheftRegion;
    safeRatioOut=1.0;
    //停止区域判断
    for(int i=0;i<pointCount;i++)
    {
        if(pointDataIn.point_z[i]>detect_min_z && pointDataIn.point_z[i]<detect_max_z)
        {
            isIn=isInRegion(pointDataIn.point_x[i],pointDataIn.point_y[i],zone_stop_x,zone_stop_y);
            isInSheftRegion=isInRegion(pointDataIn.point_x[i],pointDataIn.point_y[i],
                                       validBorder_x,validBorder_y);
//            if(true==isUseSheft)
//            {
//               isInSheftRegion=isInSheftRegionStatus(pointDataIn.point_x[i],pointDataIn.point_y[i]);
////               isInSheftRegion=isInRegion(pointDataIn.point_x[i],pointDataIn.point_y[i],sheft_border_x,sheft_border_y);
////               qDebug()<<"stop true==isUseSheft isInSheftRegion"<<isInSheftRegion;
//            }
//            else
//            {
//                isInSheftRegion=false;
//            }

            if(true==isIn && false==isInSheftRegion)//
            {
                tmpCount++;

            }
        }

    }
    if(tmpCount>=validPointNumber)
    {
        safeRatioOut=0.0;
//        qDebug()<<"safeRatioOut=0.0 tmpCount="<<tmpCount<<"pointcloudcount"<<pointCount;
        return 1;
    }

    //减速区域判断
    tmpCount=0;
    for(int i=0;i<pointCount;i++)
    {
        if(pointDataIn.point_z[i]>detect_min_z && pointDataIn.point_z[i]<detect_max_z)
        {
            isIn=isInRegion(pointDataIn.point_x[i],pointDataIn.point_y[i],zone_slow_x,zone_slow_y);
            isInSheftRegion=isInRegion(pointDataIn.point_x[i],pointDataIn.point_y[i],
                                       validBorder_x,validBorder_y);
//            if(true==isUseSheft)
//            {
//                isInSheftRegion=isInSheftRegionStatus(pointDataIn.point_x[i],pointDataIn.point_y[i]);
////               isInSheftRegion=isInRegion(pointDataIn.point_x[i],pointDataIn.point_y[i],sheft_border_x,sheft_border_y);
////               qDebug()<<"cease true==isUseSheft isInSheftRegion"<<isInSheftRegion;
//            }
//            else
//            {
//                isInSheftRegion=false;
//            }
            if(true==isIn && false==isInSheftRegion)//
            {
//                qDebug()<<"in point : pointDataIn.point_x[i]"<<pointDataIn.point_x[i]<<"pointDataIn.point_y[i]"
//                       <<pointDataIn.point_y[i];
                tmpCount++;

            }
        }

    }
    if(tmpCount>=validPointNumber)
    {
        safeRatioOut=barrierSlowDownRatio;
//        qDebug()<<"tmpCount>=validPointNumber safeRatioOut=barrierSlowDownRatio tmpCount="<<tmpCount;
        return 1;
    }


//    qDebug()<<"safeRatioOut=1 pointcloudcount"<<pointCount;
    return 1;

}

bool LaserSafetyDetect::isInRegion(double testx, double testy,std::vector<double> border_x, std::vector<double> border_y)
{
//    return false;//coredump for debug3,加上这一句后，还是会coredump，说明和下面没有关系
    //https://blog.csdn.net/u011722133/article/details/52813374

    //框选区域
    double tmpMinX=D_MAX_INTEGE;//-2^31
    double tmpMinY=D_MAX_INTEGE;
    double tmpMaxX=-D_MAX_INTEGE;
    double tmpMaxY=-D_MAX_INTEGE;
    double originMapWidth,originMapHeight;
    for(int i=0;i<border_x.size();i++)
    {
        if(border_x[i]<tmpMinX)
        {
           tmpMinX= border_x[i];
        }
        if(border_x[i]>tmpMaxX)
        {
           tmpMaxX= border_x[i];
        }
        if(border_y[i]<tmpMinY)
        {
           tmpMinY= border_y[i];
        }
        if(border_y[i]>tmpMaxY)
        {
           tmpMaxY= border_y[i];
        }
    }
    if (testx < tmpMinX || testx > tmpMaxX || testy < tmpMinY || testy > tmpMaxY)
    {
         return false;
    }
    //循环判断
    int nvert=border_x.size();
    int i, j, c = 0;
    for (i = 0, j = nvert-1; i < nvert; j = i++)
    {
        if ( ( (border_y[i]>testy) != (border_y[j]>testy) ) &&
            (testx < (border_x[j]-border_x[i]) * (testy-border_y[i]) / (border_y[j]-border_y[i]) + border_x[i]) )
        {
            c = !c;
        }

    }
    return c;




}

int LaserSafetyDetect::updateSlowDownDistance(double composedVelIn,std::vector<SlowAdjust> adjustParaIn)
{
    for(int i=0;i<adjustParaIn.size();i++)
    {
        if(composedVelIn<adjustParaIn[i].velocity)
        {
            barrierSlowDownDistance_x=adjustParaIn[i].newDistance;
            barrierSlowDownRatio=adjustParaIn[i].newSlowDownRatio;
            slowTime=adjustParaIn[i].newSlowTime;
//            qDebug()<<"detectMode="<<detectMode<<" barrierSlowDownDistance"<<barrierSlowDownDistance;
            return 1;
        }

    }
//    qDebug()<<"detectMode="<<detectMode<<" barrierSlowDownDistance"<<barrierSlowDownDistance;
    if(adjustParaIn.size()>0)
    {
        barrierSlowDownDistance_x=adjustParaIn.back().newDistance;
    }
    else
    {
        qDebug()<<"warn:adjustParaIn.size()==0";
        barrierSlowDownDistance_x=0.38;
    }

}

//bool LaserSafetyDetect::isInSheftRegionStatus(double testx, double testy)
//{
//    if(E_BARRIER_RUN_MODE_SHEFT_X==barrierRunMode)
//    {
//        return isInSheftXRegion(testx,testy);
//    }
//    else if(E_BARRIER_RUN_MODE_SHEFT_Y==barrierRunMode)
//    {
//        return isInSheftYRegion(testx,testy);
//    }
//    return false;
//}

//bool LaserSafetyDetect::isInSheftXRegion(double testx, double testy)
//{
//    return isInRegion(testx,testy, sheft_borderA_x,sheft_borderA_y);
//}

//bool LaserSafetyDetect::isInSheftYRegion(double testx, double testy)
//{
//    return isInRegion(testx,testy, sheft_borderB_x,sheft_borderB_y);
//}


int LaserSafetyDetect::readCollisionConfigFile(QString configFilePathIn, SafeCollisionInfo &collisionInfoOut
                                               ,bool &isUseNewControlMethodOut,QString &readComent)
{
    DomParser xmlParser;

    bool ok = xmlParser.openXml(configFilePathIn, QIODevice::ReadOnly );
    if( !ok )//无法打开文件！
    {
       qDebug()<<"error, LaserSafetyDetect::readConfigFile open file error！";
       return -1;
    }


    //节点ｒｅａｄ数据
   QDomNode methodNode = xmlParser.findSubNode(xmlParser.getRootDomElement(), "isUseNewControlMethod", ok );
    if( !ok )
    {

        qDebug()<<"error, LaserSafetyDetect::readConfigFile isUseNewControlMethod error！";
        readComent+=" isUseNewControlMethod";
        return -1;
    }
    xmlParser.readXml( methodNode,isUseNewControlMethodOut  );


    QDomNode  inNode;

    //-------------read SafetySetting----------
    inNode = xmlParser.findSubNode(xmlParser.getRootDomElement(), "PointCloudSafetyRatio", ok );
    if( !ok )
    {

        qDebug()<<"error, LaserSafetyDetect::readConfigFile node error！";
        return -1;
    }

    //节点ｒｅａｄ数据
    xmlParser.readXml( xmlParser.findSubNode(inNode, "barrierSlowDownDistance_x", ok),
                       collisionInfoOut.barrierSlowDownDistance_x  );
    if( !ok )
    {
        qDebug()<<"error, barrierSlowDownDistance_x node error！";
        return -1;
    }

    xmlParser.readXml( xmlParser.findSubNode(inNode, "barrierSlowDownDistance_y", ok),
                       collisionInfoOut.barrierSlowDownDistance_y  );
    if( !ok )
    {
        qDebug()<<"error, barrierSlowDownDistance_y node error！";
        return -1;
    }

    xmlParser.readXml( xmlParser.findSubNode(inNode, "barrierStopDistance_x", ok), collisionInfoOut.barrierStopDistance_x  );
    if( !ok )
    {
        qDebug()<<"error, barrierStopDistance_x node error！";
        return -1;
    }
    xmlParser.readXml( xmlParser.findSubNode(inNode, "barrierStopDistance_y", ok), collisionInfoOut.barrierStopDistance_y  );
    if( !ok )
    {
        qDebug()<<"error, barrierStopDistance_y node error！";
        return -1;
    }

    xmlParser.readXml( xmlParser.findSubNode(inNode, "detect_min_z", ok), collisionInfoOut.detect_min_z  );
    if( !ok )
    {
        qDebug()<<"error, detect_min_z node error！";
        return -1;
    }

    xmlParser.readXml( xmlParser.findSubNode(inNode, "detect_max_z", ok), collisionInfoOut.detect_max_z  );
    if( !ok )
    {
        qDebug()<<"error, detect_max_z node error！";
        return -1;
    }
    xmlParser.readXml( xmlParser.findSubNode(inNode, "pieExtendedDistance", ok), collisionInfoOut.pieExtendedDistance  );
    if( !ok )
    {
        qDebug()<<"error, pieExtendedDistance node error！";
        return -1;
    }


    //vmark common
    xmlParser.readXml( xmlParser.findSubNode(inNode, "barrierStopDistance_x_vmark", ok),
                       collisionInfoOut.barrierStopDistance_x_vmark  );
    if( !ok )
    {
        qDebug()<<"error, barrierStopDistance_x_vmark node error！";
        return -1;
    }

    xmlParser.readXml( xmlParser.findSubNode(inNode, "barrierStopDistance_y_vmark", ok),
                       collisionInfoOut.barrierStopDistance_y_vmark  );
    if( !ok )
    {
        qDebug()<<"error, barrierStopDistance_y_vmark node error！";
        return -1;
    }

    xmlParser.readXml( xmlParser.findSubNode(inNode, "barrierSlowDownDistance_x_vmark", ok),
                       collisionInfoOut.barrierSlowDownDistance_x_vmark  );
    if( !ok )
    {
        qDebug()<<"error, barrierSlowDownDistance_x_vmark node error！";
        return -1;
    }

    xmlParser.readXml( xmlParser.findSubNode(inNode, "barrierSlowDownDistance_y_vmark", ok),
                       collisionInfoOut.barrierSlowDownDistance_y_vmark  );
    if( !ok )
    {
        qDebug()<<"error, barrierSlowDownDistance_y_vmark node error！";
        return -1;
    }

    xmlParser.readXml( xmlParser.findSubNode(inNode, "pieExtendedDistance_vmark", ok),
                       collisionInfoOut.pieExtendedDistance_vmark  );
    if( !ok )
    {
        qDebug()<<"error, pieExtendedDistance_vmark node error！";
        return -1;
    }




    //narrowA common
    xmlParser.readXml( xmlParser.findSubNode(inNode, "barrierStopDistance_x_narrowA", ok),
                       collisionInfoOut.barrierStopDistance_x_narrowA  );
    if( !ok )
    {
        qDebug()<<"error, barrierStopDistance_x_narrowA node error！";
        return -1;
    }

    xmlParser.readXml( xmlParser.findSubNode(inNode, "barrierStopDistance_y_narrowA", ok),
                       collisionInfoOut.barrierStopDistance_y_narrowA  );
    if( !ok )
    {
        qDebug()<<"error, barrierStopDistance_y_narrowA node error！";
        return -1;
    }

    xmlParser.readXml( xmlParser.findSubNode(inNode, "barrierSlowDownDistance_x_narrowA", ok),
                       collisionInfoOut.barrierSlowDownDistance_x_narrowA  );
    if( !ok )
    {
        qDebug()<<"error, barrierSlowDownDistance_x_narrowA node error！";
        return -1;
    }

    xmlParser.readXml( xmlParser.findSubNode(inNode, "barrierSlowDownDistance_y_narrowA", ok),
                       collisionInfoOut.barrierSlowDownDistance_y_narrowA  );
    if( !ok )
    {
        qDebug()<<"error, barrierSlowDownDistance_y_narrowA node error！";
        return -1;
    }

    xmlParser.readXml( xmlParser.findSubNode(inNode, "pieExtendedDistance_narrowA", ok),
                       collisionInfoOut.pieExtendedDistance_narrowA  );
    if( !ok )
    {
        qDebug()<<"error, pieExtendedDistance_narrowA node error！";
        return -1;
    }




    //narrowB common
    xmlParser.readXml( xmlParser.findSubNode(inNode, "barrierStopDistance_x_narrowB", ok),
                       collisionInfoOut.barrierStopDistance_x_narrowB  );
    if( !ok )
    {
        qDebug()<<"error, barrierStopDistance_x_narrowB node error！";
        return -1;
    }

    xmlParser.readXml( xmlParser.findSubNode(inNode, "barrierStopDistance_y_narrowB", ok),
                       collisionInfoOut.barrierStopDistance_y_narrowB  );
    if( !ok )
    {
        qDebug()<<"error, barrierStopDistance_y_narrowB node error！";
        return -1;
    }

    xmlParser.readXml( xmlParser.findSubNode(inNode, "barrierSlowDownDistance_x_narrowB", ok),
                       collisionInfoOut.barrierSlowDownDistance_x_narrowB  );
    if( !ok )
    {
        qDebug()<<"error, barrierSlowDownDistance_x_narrowB node error！";
        return -1;
    }

    xmlParser.readXml( xmlParser.findSubNode(inNode, "barrierSlowDownDistance_y_narrowB", ok),
                       collisionInfoOut.barrierSlowDownDistance_y_narrowB  );
    if( !ok )
    {
        qDebug()<<"error, barrierSlowDownDistance_y_narrowB node error！";
        return -1;
    }

    xmlParser.readXml( xmlParser.findSubNode(inNode, "pieExtendedDistance_narrowB", ok),
                       collisionInfoOut.pieExtendedDistance_narrowB  );
    if( !ok )
    {
        qDebug()<<"error, pieExtendedDistance_narrowB node error！";
        return -1;
    }



    //-------------read SafetySetting----------
    inNode = xmlParser.findSubNode(xmlParser.getRootDomElement(), "StepSafetyRatioList", ok );
    if( !ok )
    {

        qDebug()<<"error, LaserSafetyDetect::readConfigFile node error！";
        return -1;
    }


    QDomNodeList devNodes = inNode.childNodes();
    SlowAdjust tmp_adjust;

    for(int i=0;i<devNodes.size();i++)
    {
        QDomNode devND =  devNodes.at(i);
        xmlParser.readXml( xmlParser.findSubNode( devND, "velocity", ok ), tmp_adjust.velocity);
        if(!ok)
        {
           return -100-i;
        }
        xmlParser.readXml( xmlParser.findSubNode( devND, "newDistance", ok ), tmp_adjust.newDistance);
        if(!ok)
        {
           return -100-i;
        }
        xmlParser.readXml( xmlParser.findSubNode( devND, "newSlowDownRatio", ok ), tmp_adjust.newSlowDownRatio);
        if(!ok)
        {
           return -100-i;
        }
        xmlParser.readXml( xmlParser.findSubNode( devND, "newSlowTime", ok ), tmp_adjust.newSlowTime);
        if(!ok)
        {
           return -100-i;
        }
//        adjustPara.append(tmp_adjust);
        collisionInfoOut.stepSafetyRatioList.push_back(tmp_adjust);
    }


    //-------------read SafetySetting vmark----------
    inNode = xmlParser.findSubNode(xmlParser.getRootDomElement(), "StepSafetyRatioList_vmark", ok );
    if( !ok )
    {

        qDebug()<<"error, LaserSafetyDetect::readConfigFile node error！";
        return -1;
    }

    devNodes = inNode.childNodes();
//    SlowAdjust tmp_adjust;

    for(int i=0;i<devNodes.size();i++)
    {
        QDomNode devND =  devNodes.at(i);
        xmlParser.readXml( xmlParser.findSubNode( devND, "velocity", ok ), tmp_adjust.velocity);
        if(!ok)
        {
           return -100-i;
        }
        xmlParser.readXml( xmlParser.findSubNode( devND, "newDistance", ok ), tmp_adjust.newDistance);
        if(!ok)
        {
           return -100-i;
        }
        xmlParser.readXml( xmlParser.findSubNode( devND, "newSlowDownRatio", ok ), tmp_adjust.newSlowDownRatio);
        if(!ok)
        {
           return -100-i;
        }
        xmlParser.readXml( xmlParser.findSubNode( devND, "newSlowTime", ok ), tmp_adjust.newSlowTime);
        if(!ok)
        {
           return -100-i;
        }
//        adjustPara.append(tmp_adjust);
        collisionInfoOut.stepSafetyRatioList_vmark.push_back(tmp_adjust);
    }


    //-------------read SafetySetting narrowA----------
    inNode = xmlParser.findSubNode(xmlParser.getRootDomElement(), "StepSafetyRatioList_narrowA", ok );
    if( !ok )
    {

        qDebug()<<"error, LaserSafetyDetect::readConfigFile node error！";
        return -1;
    }

    devNodes = inNode.childNodes();
//    SlowAdjust tmp_adjust;

    for(int i=0;i<devNodes.size();i++)
    {
        QDomNode devND =  devNodes.at(i);
        xmlParser.readXml( xmlParser.findSubNode( devND, "velocity", ok ), tmp_adjust.velocity);
        if(!ok)
        {
           return -100-i;
        }
        xmlParser.readXml( xmlParser.findSubNode( devND, "newDistance", ok ), tmp_adjust.newDistance);
        if(!ok)
        {
           return -100-i;
        }
        xmlParser.readXml( xmlParser.findSubNode( devND, "newSlowDownRatio", ok ), tmp_adjust.newSlowDownRatio);
        if(!ok)
        {
           return -100-i;
        }
        xmlParser.readXml( xmlParser.findSubNode( devND, "newSlowTime", ok ), tmp_adjust.newSlowTime);
        if(!ok)
        {
           return -100-i;
        }
//        adjustPara.append(tmp_adjust);
        collisionInfoOut.stepSafetyRatioList_narrowA.push_back(tmp_adjust);
    }


    //-------------read SafetySetting narrowB----------
    inNode = xmlParser.findSubNode(xmlParser.getRootDomElement(), "StepSafetyRatioList_narrowB", ok );
    if( !ok )
    {

        qDebug()<<"error, LaserSafetyDetect::readConfigFile node error！";
        return -1;
    }

    devNodes = inNode.childNodes();
//    SlowAdjust tmp_adjust;

    for(int i=0;i<devNodes.size();i++)
    {
        QDomNode devND =  devNodes.at(i);
        xmlParser.readXml( xmlParser.findSubNode( devND, "velocity", ok ), tmp_adjust.velocity);
        if(!ok)
        {
           return -100-i;
        }
        xmlParser.readXml( xmlParser.findSubNode( devND, "newDistance", ok ), tmp_adjust.newDistance);
        if(!ok)
        {
           return -100-i;
        }
        xmlParser.readXml( xmlParser.findSubNode( devND, "newSlowDownRatio", ok ), tmp_adjust.newSlowDownRatio);
        if(!ok)
        {
           return -100-i;
        }
        xmlParser.readXml( xmlParser.findSubNode( devND, "newSlowTime", ok ), tmp_adjust.newSlowTime);
        if(!ok)
        {
           return -100-i;
        }
//        adjustPara.append(tmp_adjust);
        collisionInfoOut.stepSafetyRatioList_narrowB.push_back(tmp_adjust);
    }


    //读取ｉｏ雷达配置
    xmlParser.readXml( xmlParser.findSubNode( xmlParser.getRootDomElement(), "isUseIoLaser", ok ), isUseIoLaser);
    if(!ok)
    {
       return -1000;
    }

    inNode = xmlParser.findSubNode(xmlParser.getRootDomElement(), "IoLaserConfig", ok );
    if( !ok )
    {

        qDebug()<<"error, LaserSafetyDetect::readConfigFile node error！";
        return -1;
    }

    devNodes = inNode.childNodes();
    IoLaserPara tmp_laserPara;
    ioLaserConfigList.clear();

    for(int i=0;i<devNodes.size();i++)
    {
        QDomNode devND =  devNodes.at(i);
        QString tmpName;
        xmlParser.readXml( xmlParser.findSubNode( devND, "laserName", ok ), tmpName);
        if(!ok)
        {
           return -100-i;
        }
        tmp_laserPara.laserName=tmpName.toStdString();
        int tmpType;
        xmlParser.readXml( xmlParser.findSubNode( devND, "laserLocationType", ok ), tmpType);
        if(!ok)
        {
           return -100-i;
        }
        tmp_laserPara.laserLocationType=(E_LOCATION_TYPE)tmpType;
        xmlParser.readXml( xmlParser.findSubNode( devND, "isLowValid", ok ), tmp_laserPara.isLowValid);
        if(!ok)
        {
           return -100-i;
        }
        xmlParser.readXml( xmlParser.findSubNode( devND, "laserOutputCount", ok ), tmp_laserPara.laserOutputCount);
        if(!ok)
        {
           return -100-i;
        }
        xmlParser.readXml( xmlParser.findSubNode( devND, "laserDi1_index", ok ), tmp_laserPara.laserDi1_index);
        if(!ok)
        {
           return -100-i;
        }
        xmlParser.readXml( xmlParser.findSubNode( devND, "laserDi2_index", ok ), tmp_laserPara.laserDi2_index);
        if(!ok)
        {
           return -100-i;
        }
        xmlParser.readXml( xmlParser.findSubNode( devND, "laserDi3_index", ok ), tmp_laserPara.laserDi3_index);
        if(!ok)
        {
           return -100-i;
        }
        xmlParser.readXml( xmlParser.findSubNode( devND, "laserDi4_index", ok ), tmp_laserPara.laserDi4_index);
        if(!ok)
        {
           return -100-i;
        }
        xmlParser.readXml( xmlParser.findSubNode( devND, "laserDo1_index", ok ), tmp_laserPara.laserDo1_index);
        if(!ok)
        {
           return -100-i;
        }
        xmlParser.readXml( xmlParser.findSubNode( devND, "laserDo2_index", ok ), tmp_laserPara.laserDo2_index);
        if(!ok)
        {
           return -100-i;
        }
        xmlParser.readXml( xmlParser.findSubNode( devND, "laserDo3_index", ok ), tmp_laserPara.laserDo3_index);
        if(!ok)
        {
           return -100-i;
        }
        xmlParser.readXml( xmlParser.findSubNode( devND, "laserDo4_index", ok ), tmp_laserPara.laserDo4_index);
        if(!ok)
        {
           return -100-i;
        }

        xmlParser.readXml( xmlParser.findSubNode( devND, "isRotateValid", ok ), tmp_laserPara.isRotateValid);
        if(!ok)
        {
           readComent+=" isRotateValid";
           return -100-i;
        }

//        adjustPara.append(tmp_adjust);
        ioLaserConfigList.push_back(tmp_laserPara);
    }


    xmlParser.closeXml( );

    return 1;
}

int LaserSafetyDetect::writeCollisionConfigFile(QString configFilePathIn, SafeCollisionInfo collisionInfoIn)
{
    DomParser xmlParser;

    bool ok = xmlParser.openXml(configFilePathIn, QIODevice::ReadWrite );
    if( !ok )//无法打开文件！
    {
       qDebug()<<"error, LaserSafetyDetect::writeCollisionConfigFile open file error！";
       return -1;
    }

    QDomNode  inNode;

    //-------------read SafetySetting----------
    inNode = xmlParser.findSubNode(xmlParser.getRootDomElement(), "PointCloudSafetyRatio", ok );
    if( !ok )
    {

        qDebug()<<"error, LaserSafetyDetect::readConfigFile node error！";
        return -1;
    }


    //common
    xmlParser.writeXml( xmlParser.findSubNode(inNode, "barrierSlowDownDistance_x", ok),
                        collisionInfoIn.barrierSlowDownDistance_x  );
    if( !ok )
    {
        qDebug()<<"error, barrierSlowDownDistance_x node error！";
        return -1;
    }

    xmlParser.writeXml( xmlParser.findSubNode(inNode, "barrierSlowDownDistance_y", ok),
                        collisionInfoIn.barrierSlowDownDistance_y  );
    if( !ok )
    {
        qDebug()<<"error, barrierSlowDownDistance_y node error！";
        return -1;
    }


    xmlParser.writeXml( xmlParser.findSubNode(inNode, "barrierStopDistance_x", ok), collisionInfoIn.barrierStopDistance_x  );
    if( !ok )
    {
        qDebug()<<"error, barrierStopDistance node error！";
        return -1;
    }
    xmlParser.writeXml( xmlParser.findSubNode(inNode, "barrierStopDistance_y", ok), collisionInfoIn.barrierStopDistance_y  );
    if( !ok )
    {
        qDebug()<<"error, barrierStopDistance node error！";
        return -1;
    }

    xmlParser.writeXml( xmlParser.findSubNode(inNode, "detect_min_z", ok), collisionInfoIn.detect_min_z  );
    if( !ok )
    {
        qDebug()<<"error, detect_min_z node error！";
        return -1;
    }

    xmlParser.writeXml( xmlParser.findSubNode(inNode, "detect_max_z", ok), collisionInfoIn.detect_max_z  );
    if( !ok )
    {
        qDebug()<<"error, detect_max_z node error！";
        return -1;
    }
    xmlParser.writeXml( xmlParser.findSubNode(inNode, "pieExtendedDistance", ok), collisionInfoIn.pieExtendedDistance  );
    if( !ok )
    {
        qDebug()<<"error, pieExtendedDistance node error！";
        return -1;
    }

    //vmark common
    xmlParser.writeXml( xmlParser.findSubNode(inNode, "barrierStopDistance_x_vmark", ok), collisionInfoIn.barrierStopDistance_x_vmark  );
    if( !ok )
    {
        qDebug()<<"error, barrierStopDistance_x_vmark node error！";
        return -1;
    }

    xmlParser.writeXml( xmlParser.findSubNode(inNode, "barrierStopDistance_y_vmark", ok),
                       collisionInfoIn.barrierStopDistance_y_vmark  );
    if( !ok )
    {
        qDebug()<<"error, barrierStopDistance_y_vmark node error！";
        return -1;
    }

    xmlParser.writeXml( xmlParser.findSubNode(inNode, "barrierSlowDownDistance_x_vmark", ok),
                       collisionInfoIn.barrierSlowDownDistance_x_vmark  );
    if( !ok )
    {
        qDebug()<<"error, barrierSlowDownDistance_x_vmark node error！";
        return -1;
    }

    xmlParser.writeXml( xmlParser.findSubNode(inNode, "barrierSlowDownDistance_y_vmark", ok),
                       collisionInfoIn.barrierSlowDownDistance_y_vmark  );
    if( !ok )
    {
        qDebug()<<"error, barrierSlowDownDistance_y_vmark node error！";
        return -1;
    }


    xmlParser.writeXml( xmlParser.findSubNode(inNode, "pieExtendedDistance_vmark", ok),
                       collisionInfoIn.pieExtendedDistance_vmark  );
    if( !ok )
    {
        qDebug()<<"error, pieExtendedDistance_vmark node error！";
        return -1;
    }



    //narrowA common
    xmlParser.writeXml( xmlParser.findSubNode(inNode, "barrierStopDistance_x_narrowA", ok),
                        collisionInfoIn.barrierStopDistance_x_narrowA  );
    if( !ok )
    {
        qDebug()<<"error, barrierStopDistance_x_narrowA node error！";
        return -1;
    }

    xmlParser.writeXml( xmlParser.findSubNode(inNode, "barrierStopDistance_y_narrowA", ok),
                       collisionInfoIn.barrierStopDistance_y_narrowA  );
    if( !ok )
    {
        qDebug()<<"error, barrierStopDistance_y_narrowA node error！";
        return -1;
    }

    xmlParser.writeXml( xmlParser.findSubNode(inNode, "barrierSlowDownDistance_x_narrowA", ok),
                       collisionInfoIn.barrierSlowDownDistance_x_narrowA  );
    if( !ok )
    {
        qDebug()<<"error, barrierSlowDownDistance_x_narrowA node error！";
        return -1;
    }

    xmlParser.writeXml( xmlParser.findSubNode(inNode, "barrierSlowDownDistance_y_narrowA", ok),
                       collisionInfoIn.barrierSlowDownDistance_y_narrowA  );
    if( !ok )
    {
        qDebug()<<"error, barrierSlowDownDistance_y_narrowA node error！";
        return -1;
    }


    xmlParser.writeXml( xmlParser.findSubNode(inNode, "pieExtendedDistance_narrowA", ok),
                       collisionInfoIn.pieExtendedDistance_narrowA  );
    if( !ok )
    {
        qDebug()<<"error, pieExtendedDistance_narrowA node error！";
        return -1;
    }



    //narrowB common
    xmlParser.writeXml( xmlParser.findSubNode(inNode, "barrierStopDistance_x_narrowB", ok),
                        collisionInfoIn.barrierStopDistance_x_narrowB  );
    if( !ok )
    {
        qDebug()<<"error, barrierStopDistance_x_narrowB node error！";
        return -1;
    }

    xmlParser.writeXml( xmlParser.findSubNode(inNode, "barrierStopDistance_y_narrowB", ok),
                       collisionInfoIn.barrierStopDistance_y_narrowB  );
    if( !ok )
    {
        qDebug()<<"error, barrierStopDistance_y_narrowB node error！";
        return -1;
    }

    xmlParser.writeXml( xmlParser.findSubNode(inNode, "barrierSlowDownDistance_x_narrowB", ok),
                       collisionInfoIn.barrierSlowDownDistance_x_narrowB  );
    if( !ok )
    {
        qDebug()<<"error, barrierSlowDownDistance_x_narrowB node error！";
        return -1;
    }

    xmlParser.writeXml( xmlParser.findSubNode(inNode, "barrierSlowDownDistance_y_narrowB", ok),
                       collisionInfoIn.barrierSlowDownDistance_y_narrowB  );
    if( !ok )
    {
        qDebug()<<"error, barrierSlowDownDistance_y_narrowB node error！";
        return -1;
    }


    xmlParser.writeXml( xmlParser.findSubNode(inNode, "pieExtendedDistance_narrowB", ok),
                       collisionInfoIn.pieExtendedDistance_narrowB  );
    if( !ok )
    {
        qDebug()<<"error, pieExtendedDistance_narrowB node error！";
        return -1;
    }


    //-------------writeXml SafetySetting----------
    inNode = xmlParser.findSubNode(xmlParser.getRootDomElement(), "StepSafetyRatioList", ok );
    if( !ok )
    {

        qDebug()<<"error, LaserSafetyDetect::readConfigFile node error！";
        return -1;
    }


    QDomNodeList devNodes = inNode.childNodes();
    SlowAdjust tmp_adjust;

    for(int i=0;i<devNodes.size();i++)
    {
        if(collisionInfoIn.stepSafetyRatioList.size()>i)
        {
            QDomNode devND =  devNodes.at(i);
            xmlParser.writeXml( xmlParser.findSubNode( devND, "velocity", ok ),
                                collisionInfoIn.stepSafetyRatioList[i].velocity);
            if(!ok)
            {
               return -100-i;
            }
            xmlParser.writeXml( xmlParser.findSubNode( devND, "newDistance", ok ),
                                collisionInfoIn.stepSafetyRatioList[i].newDistance);
            if(!ok)
            {
               return -100-i;
            }
            xmlParser.writeXml( xmlParser.findSubNode( devND, "newSlowDownRatio", ok ),
                                collisionInfoIn.stepSafetyRatioList[i].newSlowDownRatio);
            if(!ok)
            {
               return -100-i;
            }
        }

    }


    //-------------writeXml SafetySetting vmark----------
    inNode = xmlParser.findSubNode(xmlParser.getRootDomElement(), "StepSafetyRatioList_vmark", ok );
    if( !ok )
    {

        qDebug()<<"error, LaserSafetyDetect::readConfigFile node error！";
        return -1;
    }


    devNodes = inNode.childNodes();

    for(int i=0;i<devNodes.size();i++)
    {
        if(collisionInfoIn.stepSafetyRatioList_vmark.size()>i)
        {
            QDomNode devND =  devNodes.at(i);
            xmlParser.writeXml( xmlParser.findSubNode( devND, "velocity", ok ),
                                collisionInfoIn.stepSafetyRatioList_vmark[i].velocity);
            if(!ok)
            {
               return -100-i;
            }
            xmlParser.writeXml( xmlParser.findSubNode( devND, "newDistance", ok ),
                                collisionInfoIn.stepSafetyRatioList_vmark[i].newDistance);
            if(!ok)
            {
               return -100-i;
            }
            xmlParser.writeXml( xmlParser.findSubNode( devND, "newSlowDownRatio", ok ),
                                collisionInfoIn.stepSafetyRatioList_vmark[i].newSlowDownRatio);
            if(!ok)
            {
               return -100-i;
            }
        }

    }


    //-------------writeXml SafetySetting narrowA----------
    inNode = xmlParser.findSubNode(xmlParser.getRootDomElement(), "StepSafetyRatioList_narrowA", ok );
    if( !ok )
    {

        qDebug()<<"error, LaserSafetyDetect::readConfigFile node error！";
        return -1;
    }


    devNodes = inNode.childNodes();

    for(int i=0;i<devNodes.size();i++)
    {
        if(collisionInfoIn.stepSafetyRatioList_narrowA.size()>i)
        {
            QDomNode devND =  devNodes.at(i);
            xmlParser.writeXml( xmlParser.findSubNode( devND, "velocity", ok ),
                                collisionInfoIn.stepSafetyRatioList_narrowA[i].velocity);
            if(!ok)
            {
               return -100-i;
            }
            xmlParser.writeXml( xmlParser.findSubNode( devND, "newDistance", ok ),
                                collisionInfoIn.stepSafetyRatioList_narrowA[i].newDistance);
            if(!ok)
            {
               return -100-i;
            }
            xmlParser.writeXml( xmlParser.findSubNode( devND, "newSlowDownRatio", ok ),
                                collisionInfoIn.stepSafetyRatioList_narrowA[i].newSlowDownRatio);
            if(!ok)
            {
               return -100-i;
            }
        }

    }



    //-------------writeXml SafetySetting narrowB----------
    inNode = xmlParser.findSubNode(xmlParser.getRootDomElement(), "StepSafetyRatioList_narrowB", ok );
    if( !ok )
    {

        qDebug()<<"error, LaserSafetyDetect::readConfigFile node error！";
        return -1;
    }


    devNodes = inNode.childNodes();

    for(int i=0;i<devNodes.size();i++)
    {
        if(collisionInfoIn.stepSafetyRatioList_narrowB.size()>i)
        {
            QDomNode devND =  devNodes.at(i);
            xmlParser.writeXml( xmlParser.findSubNode( devND, "velocity", ok ),
                                collisionInfoIn.stepSafetyRatioList_narrowB[i].velocity);
            if(!ok)
            {
               return -100-i;
            }
            xmlParser.writeXml( xmlParser.findSubNode( devND, "newDistance", ok ),
                                collisionInfoIn.stepSafetyRatioList_narrowB[i].newDistance);
            if(!ok)
            {
               return -100-i;
            }
            xmlParser.writeXml( xmlParser.findSubNode( devND, "newSlowDownRatio", ok ),
                                collisionInfoIn.stepSafetyRatioList_narrowB[i].newSlowDownRatio);
            if(!ok)
            {
               return -100-i;
            }
        }

    }



    xmlParser.closeXml( true );

    return 1;
}

int LaserSafetyDetect::initialCollisionParameter(SafeCollisionInfo collisionInfoIn)
{
    detect_min_z = collisionInfoIn.detect_min_z;
    detect_max_z = collisionInfoIn.detect_max_z;
    barrierSlowDownRatio =0.3;
    //common
    barrierSlowDownDistance_x = collisionInfoIn.barrierSlowDownDistance_x;
    barrierSlowDownDistance_y = collisionInfoIn.barrierSlowDownDistance_y;
    barrierStopDistance_x = collisionInfoIn.barrierStopDistance_x;
    barrierStopDistance_y = collisionInfoIn.barrierStopDistance_y;
    pieExtendedDistance = collisionInfoIn.pieExtendedDistance;
    adjustPara=collisionInfoIn.stepSafetyRatioList;
    //vmark
    barrierSlowDownDistance_x_vmark = collisionInfoIn.barrierSlowDownDistance_x_vmark;
    barrierSlowDownDistance_y_vmark = collisionInfoIn.barrierSlowDownDistance_y_vmark;
    barrierStopDistance_x_vmark = collisionInfoIn.barrierStopDistance_x_vmark;
    barrierStopDistance_y_vmark = collisionInfoIn.barrierStopDistance_y_vmark;
    pieExtendedDistance_vmark = collisionInfoIn.pieExtendedDistance_vmark;
    adjustPara_vmark=collisionInfoIn.stepSafetyRatioList_vmark;

    //narrowA
    barrierSlowDownDistance_x_narrowA = collisionInfoIn.barrierSlowDownDistance_x_narrowA;
    barrierSlowDownDistance_y_narrowA = collisionInfoIn.barrierSlowDownDistance_y_narrowA;
    barrierStopDistance_x_narrowA = collisionInfoIn.barrierStopDistance_x_narrowA;
    barrierStopDistance_y_narrowA = collisionInfoIn.barrierStopDistance_y_narrowA;
    pieExtendedDistance_narrowA = collisionInfoIn.pieExtendedDistance_narrowA;
    adjustPara_narrowA=collisionInfoIn.stepSafetyRatioList_narrowA;

    //narrowB
    barrierSlowDownDistance_x_narrowB = collisionInfoIn.barrierSlowDownDistance_x_narrowB;
    barrierSlowDownDistance_y_narrowB= collisionInfoIn.barrierSlowDownDistance_y_narrowB;
    barrierStopDistance_x_narrowB = collisionInfoIn.barrierStopDistance_x_narrowB;
    barrierStopDistance_y_narrowB = collisionInfoIn.barrierStopDistance_y_narrowB;
    pieExtendedDistance_narrowB = collisionInfoIn.pieExtendedDistance_narrowB;
    adjustPara_narrowB=collisionInfoIn.stepSafetyRatioList_narrowB;
    return 1;
}

int LaserSafetyDetect::readCarBorderFile(QString configFilePathIn)
{

    DomParser xmlParser;

    bool ok = xmlParser.openXml(configFilePathIn, QIODevice::ReadOnly );
    if( !ok )//无法打开文件！
    {
       qDebug()<<"error, setVehicleBorder open file error！";
       return -1;
    }

    //车边框读取
    QDomNode  inNode = xmlParser.findSubNode(xmlParser.getRootDomElement(), "BasicSetting", ok );
    if( !ok )
    {

        qDebug()<<"error, readCarBorderFile node error！";
        return -1;
    }

    QDomNodeList devNodes = inNode.childNodes();

    origin_car_border_x.resize(devNodes.size());
    origin_car_border_y.resize(devNodes.size());

    for(int i=0;i<devNodes.size();i++)
    {
        QDomNode devND =  devNodes.at(i);
        xmlParser.readXml( xmlParser.findSubNode( devND, "x_position", ok ), origin_car_border_x[i] );
        if(!ok)
        {
           return -100-i;
        }
        xmlParser.readXml( xmlParser.findSubNode( devND, "y_position", ok ), origin_car_border_y[i] );
        if(!ok)
        {
           return -100-i;
        }
    }



    inNode = xmlParser.findSubNode(xmlParser.getRootDomElement(), "OffsetSetting", ok );
    //节点ｒｅａｄ数据
    xmlParser.readXml( xmlParser.findSubNode(inNode, "front_offset", ok), front_offset  );
    if( !ok )
    {
        qDebug()<<"error, readCarBorderFile node error！";
        return -1;
    }
    xmlParser.readXml( xmlParser.findSubNode(inNode, "back_offset", ok), back_offset  );
    if( !ok )
    {
        qDebug()<<"error, readCarBorderFile node error！";
        return -1;
    }
    xmlParser.readXml( xmlParser.findSubNode(inNode, "left_offset", ok), left_offset  );
    if( !ok )
    {
        qDebug()<<"error, readCarBorderFile node error！";
        return -1;
    }
    xmlParser.readXml( xmlParser.findSubNode(inNode, "right_offset", ok), right_offset  );
    if( !ok )
    {
        qDebug()<<"error, readCarBorderFile node error！";
        return -1;
    }

    //料架a边框读取
    inNode = xmlParser.findSubNode(xmlParser.getRootDomElement(), "SheftBorderA", ok );
    if( !ok )
    {

        qDebug()<<"error, readCarBorderFile node error！";
        return -1;
    }

    devNodes = inNode.childNodes();

    sheft_borderA_x.resize(devNodes.size());
    sheft_borderA_y.resize(devNodes.size());

    for(int i=0;i<devNodes.size();i++)
    {
        QDomNode devND =  devNodes.at(i);
        xmlParser.readXml( xmlParser.findSubNode( devND, "x_position", ok ), sheft_borderA_x[i] );
        if(!ok)
        {
           return -100-i;
        }
        xmlParser.readXml( xmlParser.findSubNode( devND, "y_position", ok ), sheft_borderA_y[i] );
        if(!ok)
        {
           return -100-i;
        }
    }

    //料架b边框读取
    inNode = xmlParser.findSubNode(xmlParser.getRootDomElement(), "SheftBorderB", ok );
    if( !ok )
    {

        qDebug()<<"error, readCarBorderFile node error！";
        return -1;
    }

    devNodes = inNode.childNodes();

    sheft_borderB_x.resize(devNodes.size());
    sheft_borderB_y.resize(devNodes.size());

    for(int i=0;i<devNodes.size();i++)
    {
        QDomNode devND =  devNodes.at(i);
        xmlParser.readXml( xmlParser.findSubNode( devND, "x_position", ok ), sheft_borderB_x[i] );
        if(!ok)
        {
           return -100-i;
        }
        xmlParser.readXml( xmlParser.findSubNode( devND, "y_position", ok ), sheft_borderB_y[i] );
        if(!ok)
        {
           return -100-i;
        }
    }

//    inNode = xmlParser.findSubNode(xmlParser.getRootDomElement(), "isUseSheftBorder", ok );
//    if( !ok )
//    {

//        qDebug()<<"error, readCarBorderFile node error！";
//        return -1;
//    }
//    int tmpResult;
//    xmlParser.readXml( inNode, tmpResult);
//    isUseSheft=tmpResult;


    xmlParser.closeXml();

    return 1;
}


int LaserSafetyDetect::saveVehicleBorderFile_offset(QString filePathIn)
{
    DomParser xmlParser;

    bool ok = xmlParser.openXml(filePathIn, QIODevice::ReadWrite );
    if( !ok )//无法打开文件！
    {
       qDebug()<<"error, saveVehicleBorderFile open file error！";
       return -1;
    }

    QDomNode  inNode ;


    inNode = xmlParser.findSubNode(xmlParser.getRootDomElement(), "OffsetSetting", ok );
    if( !ok )
    {
        qDebug()<<"error, saveVehicleBorderFile node error！";
        return -1;
    }
    //节点ｒｅａｄ数据
    xmlParser.writeXml( xmlParser.findSubNode(inNode, "front_offset", ok), front_offset  );
    if( !ok )
    {
        qDebug()<<"error, saveVehicleBorderFile node error！";
        return -1;
    }
    xmlParser.writeXml( xmlParser.findSubNode(inNode, "back_offset", ok), back_offset  );
    if( !ok )
    {
        qDebug()<<"error, saveVehicleBorderFile node error！";
        return -1;
    }
    xmlParser.writeXml( xmlParser.findSubNode(inNode, "left_offset", ok), left_offset  );
    if( !ok )
    {
        qDebug()<<"error, saveVehicleBorderFile node error！";
        return -1;
    }
    xmlParser.writeXml( xmlParser.findSubNode(inNode, "right_offset", ok), right_offset  );
    if( !ok )
    {
        qDebug()<<"error, saveVehicleBorderFile node error！";
        return -1;
    }

    xmlParser.closeXml(true);

    return 1;
}


void LaserSafetyDetect::addMsg(int messageLevel, std::string componentName, std::string messageType
                              , int messageCode, int robotIdIn
                              , double parameter1, double parameter2, int parameter3, int parameter4
                               ,QString comment)
{
    Message tmpMsg;
    tmpMsg.MessageLevel = messageLevel;
    tmpMsg.componentClassName = componentName;
    tmpMsg.messageCode = messageCode;
    tmpMsg.messageType = messageType;
    tmpMsg.robotId = robotIdIn;

    string infomationStr;
    QString tmpStr;

    switch(messageCode)
    {

    case 8141:
    {
        tmpStr =  QObject::tr("机器人:")  + QString::number(robotId);
        tmpStr = tmpStr + QObject::tr(",pointCloudSafeRatio.xml文件读取失败　！")+comment;
        infomationStr = tmpStr.toStdString();
        break;
    }
    case 8140:
    {
        tmpStr =  QObject::tr("机器人:")  + QString::number(robotId);
        tmpStr = tmpStr + QObject::tr(",carBorder.xml文件读取失败　！");
        infomationStr = tmpStr.toStdString();
        break;
    }



        default:
        {
            qDebug()<<"Coordinate::addMsg====not match error code";
            break;
        }
    }

    tmpMsg.MessageInformation = infomationStr;

    #ifndef MOTION_SERVER_UNIT_TEST
    motionMessage->addMsg(tmpMsg);
    #endif
}
